#include <cstdlib>
#include <iostream>
#include <complex>
#include "testlib/testlib_test.h"

#include "vnl/vnl_diag_matrix.h"
#include "vnl/vnl_complexify.h"
#include "vnl/vnl_real.h"
#include "vnl/vnl_imag.h"


template <typename ValueType>
static void
test_complexify_instance(const ValueType & re, const ValueType & im)
{

  constexpr unsigned int length = 2;

  std::cout << "##################" << std::endl;
  std::cout << "Testing vnl_vector" << std::endl;
  std::cout << "##################" << std::endl;

  vnl_vector<ValueType> r_vector(length, re);
  vnl_vector<ValueType> i_vector(length, im);
  vnl_vector<std::complex<ValueType>> c_vector = vnl_complexify(r_vector);
  for (unsigned int i = 0; i < length; ++i)
  {
    TEST("vnl_vector real", re, c_vector.get(i).real());
    TEST("vnl_vector imag", 0.0, c_vector.get(i).imag());
  }
  c_vector = vnl_complexify(r_vector, i_vector);
  for (unsigned int i = 0; i < length; ++i)
  {
    TEST("vnl_vector real", re, c_vector.get(i).real());
    TEST("vnl_vector imag", im, c_vector.get(i).imag());
  }
  r_vector = vnl_imag(c_vector);
  i_vector = vnl_real(c_vector);
  for (unsigned int i = 0; i < length; ++i)
  {
    TEST("vnl_vector vnl_real", re, i_vector.get(i));
    TEST("vnl_vector vnl_imag", im, r_vector.get(i));
  }

  std::cout << "########################" << std::endl;
  std::cout << "Testing vnl_vector_fixed" << std::endl;
  std::cout << "########################" << std::endl;

  vnl_vector_fixed<ValueType, length> r_vector_fixed(re);
  vnl_vector_fixed<ValueType, length> i_vector_fixed(im);
  vnl_vector_fixed<std::complex<ValueType>, length> c_vector_fixed = vnl_complexify(r_vector_fixed);
  for (unsigned int i = 0; i < length; ++i)
  {
    TEST("vnl_vector_fixed real", re, c_vector_fixed.get(i).real());
    TEST("vnl_vector_fixed imag", 0.0, c_vector_fixed.get(i).imag());
  }
  c_vector_fixed = vnl_complexify(r_vector_fixed, i_vector_fixed);
  for (unsigned int i = 0; i < length; ++i)
  {
    TEST("vnl_vector_fixed real", re, c_vector_fixed.get(i).real());
    TEST("vnl_vector_fixed imag", im, c_vector_fixed.get(i).imag());
  }
  r_vector_fixed = vnl_imag(c_vector_fixed);
  i_vector_fixed = vnl_real(c_vector_fixed);
  for (unsigned int i = 0; i < length; ++i)
  {
    TEST("vnl_vector_fixed vnl_real", re, i_vector_fixed.get(i));
    TEST("vnl_vector_fixed vnl_imag", im, r_vector_fixed.get(i));
  }

  std::cout << "##################" << std::endl;
  std::cout << "Testing vnl_matrix" << std::endl;
  std::cout << "##################" << std::endl;

  vnl_matrix<ValueType> r_matrix(length, length, re);
  vnl_matrix<ValueType> i_matrix(length, length, im);
  vnl_matrix<std::complex<ValueType>> c_matrix = vnl_complexify(r_matrix);
  for (unsigned int c = 0; c < length; ++c)
  {
    for (unsigned int r = 0; r < length; ++r)
    {
      TEST("vnl_matrix real", re, c_matrix.get(r, c).real());
      TEST("vnl_matrix imag", 0.0, c_matrix.get(r, c).imag());
    }
  }
  c_matrix = vnl_complexify(r_matrix, i_matrix);
  for (unsigned int c = 0; c < length; ++c)
  {
    for (unsigned int r = 0; r < length; ++r)
    {
      TEST("vnl_matrix real", re, c_matrix.get(r, c).real());
      TEST("vnl_matrix imag", im, c_matrix.get(r, c).imag());
    }
  }
  r_matrix = vnl_imag(c_matrix);
  i_matrix = vnl_real(c_matrix);
  for (unsigned int c = 0; c < length; ++c)
  {
    for (unsigned int r = 0; r < length; ++r)
    {
      TEST("vnl_matrix vnl_real", re, i_matrix.get(r, c));
      TEST("vnl_matrix vnl_imag", im, r_matrix.get(r, c));
    }
  }

  std::cout << "########################" << std::endl;
  std::cout << "Testing vnl_matrix_fixed" << std::endl;
  std::cout << "########################" << std::endl;

  vnl_matrix_fixed<ValueType, length, length> r_matrix_fixed(re);
  vnl_matrix_fixed<ValueType, length, length> i_matrix_fixed(im);
  vnl_matrix_fixed<std::complex<ValueType>, length, length> c_matrix_fixed = vnl_complexify(r_matrix_fixed);
  for (unsigned int c = 0; c < length; ++c)
  {
    for (unsigned int r = 0; r < length; ++r)
    {
      TEST("vnl_matrix_fixed real", re, c_matrix_fixed.get(r, c).real());
      TEST("vnl_matrix_fixed imag", 0.0, c_matrix_fixed.get(r, c).imag());
    }
  }
  c_matrix_fixed = vnl_complexify(r_matrix_fixed, i_matrix_fixed);
  for (unsigned int c = 0; c < length; ++c)
  {
    for (unsigned int r = 0; r < length; ++r)
    {
      TEST("vnl_matrix_fixed real", re, c_matrix_fixed.get(r, c).real());
      TEST("vnl_matrix_fixed imag", im, c_matrix_fixed.get(r, c).imag());
    }
  }
  r_matrix_fixed = vnl_imag(c_matrix_fixed);
  i_matrix_fixed = vnl_real(c_matrix_fixed);
  for (unsigned int c = 0; c < length; ++c)
  {
    for (unsigned int r = 0; r < length; ++r)
    {
      TEST("vnl_matrix_fixed vnl_real", re, i_matrix_fixed.get(r, c));
      TEST("vnl_matrix_fixed vnl_imag", im, r_matrix_fixed.get(r, c));
    }
  }

  std::cout << "#######################" << std::endl;
  std::cout << "Testing vnl_diag_matrix" << std::endl;
  std::cout << "#######################" << std::endl;

  vnl_diag_matrix<ValueType> r_diag_matrix(length, re);
  vnl_diag_matrix<ValueType> i_diag_matrix(length, im);
  vnl_diag_matrix<std::complex<ValueType>> c_diag_matrix = vnl_complexify(r_diag_matrix);
  for (unsigned int i = 0; i < length; ++i)
  {
    TEST("vnl_diag_matrix real", re, c_diag_matrix.get(i, i).real());
    TEST("vnl_diag_matrix imag", 0.0, c_diag_matrix.get(i, i).imag());
  }
  c_diag_matrix = vnl_complexify(r_diag_matrix, i_diag_matrix);
  for (unsigned int i = 0; i < length; ++i)
  {
    TEST("vnl_diag_matrix real", re, c_diag_matrix.get(i, i).real());
    TEST("vnl_diag_matrix imag", im, c_diag_matrix.get(i, i).imag());
  }
  r_diag_matrix = vnl_imag(c_diag_matrix);
  i_diag_matrix = vnl_real(c_diag_matrix);
  for (unsigned int i = 0; i < length; ++i)
  {
    TEST("vnl_diag_matrix vnl_real", re, i_diag_matrix.get(i, i));
    TEST("vnl_diag_matrix vnl_imag", im, r_diag_matrix.get(i, i));
  }

  std::cout << "#############################" << std::endl;
  std::cout << "Testing vnl_diag_matrix_fixed" << std::endl;
  std::cout << "#############################" << std::endl;

  vnl_diag_matrix_fixed<ValueType, length> r_diag_matrix_fixed(re);
  vnl_diag_matrix_fixed<ValueType, length> i_diag_matrix_fixed(im);
  vnl_diag_matrix_fixed<std::complex<ValueType>, length> c_diag_matrix_fixed = vnl_complexify(r_diag_matrix_fixed);
  for (unsigned int i = 0; i < length; ++i)
  {
    TEST("vnl_diag_matrix_fixed real", re, c_diag_matrix_fixed.get(i, i).real());
    TEST("vnl_diag_matrix_fixed imag", 0.0, c_diag_matrix_fixed.get(i, i).imag());
  }
  c_diag_matrix_fixed = vnl_complexify(r_diag_matrix_fixed, i_diag_matrix_fixed);
  for (unsigned int i = 0; i < length; ++i)
  {
    TEST("vnl_diag_matrix_fixed real", re, c_diag_matrix_fixed.get(i, i).real());
    TEST("vnl_diag_matrix_fixed imag", im, c_diag_matrix_fixed.get(i, i).imag());
  }
  r_diag_matrix_fixed = vnl_imag(c_diag_matrix_fixed);
  i_diag_matrix_fixed = vnl_real(c_diag_matrix_fixed);
  for (unsigned int i = 0; i < length; ++i)
  {
    TEST("vnl_diag_matrix_fixed vnl_real", re, i_diag_matrix_fixed.get(i, i));
    TEST("vnl_diag_matrix_fixed vnl_imag", im, r_diag_matrix_fixed.get(i, i));
  }

  std::cout << "######################" << std::endl;
  std::cout << "Testing vnl_sym_matrix" << std::endl;
  std::cout << "######################" << std::endl;

  vnl_sym_matrix<ValueType> r_sym_matrix(length, re);
  vnl_sym_matrix<ValueType> i_sym_matrix(length, im);
  vnl_sym_matrix<std::complex<ValueType>> c_sym_matrix = vnl_complexify(r_sym_matrix);
  for (unsigned int c = 0; c < length; ++c)
  {
    for (unsigned int r = 0; r < length; ++r)
    {
      TEST("vnl_sym_matrix real", re, c_sym_matrix(r, c).real()); // no get()
      TEST("vnl_sym_matrix imag", 0.0, c_sym_matrix(r, c).imag());
    }
  }
  c_sym_matrix = vnl_complexify(r_sym_matrix, i_sym_matrix);
  for (unsigned int c = 0; c < length; ++c)
  {
    for (unsigned int r = 0; r < length; ++r)
    {
      TEST("vnl_sym_matrix real", re, c_sym_matrix(r, c).real()); // no get()
      TEST("vnl_sym_matrix imag", im, c_sym_matrix(r, c).imag());
    }
  }
  r_sym_matrix = vnl_imag(c_sym_matrix);
  i_sym_matrix = vnl_real(c_sym_matrix);
  for (unsigned int c = 0; c < length; ++c)
  {
    for (unsigned int r = 0; r < length; ++r)
    {
      TEST("vnl_sym_matrix vnl_real", re, i_sym_matrix(r, c));
      TEST("vnl_sym_matrix vnl_imag", im, r_sym_matrix(r, c));
    }
  }
}

static void
test_complexify()
{
  test_complexify_instance<float>(7.5, 8.5);
  test_complexify_instance<double>(7.5, 8.5);
}

TESTMAIN(test_complexify);
