#define BOOST_TEST_MAIN
#define BOOST_TEST_MODULE davidson_test

#include <boost/test/unit_test.hpp>

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <Eigen/QR>
#include <Eigen/Eigenvalues>
#include <chrono>

#include "../src/DavidsonSolver.hpp"
#include "../src/DavidsonOperator.hpp"
#include "../src/MatrixFreeOperator.hpp"

// intiialize a full matrix
Eigen::MatrixXd init_matrix(int N, double eps, bool diag)
{
    Eigen::MatrixXd matrix;
    matrix =  eps * Eigen::MatrixXd::Random(N,N);
    Eigen::MatrixXd tmat = matrix.transpose();
    matrix = matrix + tmat;

    for (int i = 0; i<N; i++) {
        if(diag)    matrix(i,i) = static_cast<double> (i+1);
        else        matrix(i,i) =  static_cast<double> (1. + (std::rand() %1000 ) / 10.);
    }
    return matrix;
}

// test to derived the matrix free operator class
class TestOperator : public MatrixFreeOperator
{
    public :
    TestOperator(int n) {_size = n;}
    Eigen::VectorXd col(int index) const;
};

//  get a col of the operator
Eigen::VectorXd TestOperator::col(int index) const
{
    Eigen::VectorXd col_out = Eigen::VectorXd::Zero(_size,1);
    for (int j=0; j < _size; j++)
    {
        if (j==index) {
            col_out(j) = static_cast<double> (j+1);
        }
        else{
            col_out(j) = 0.01 / std::pow( static_cast<double>(j-index),2) ;
        }
    }
    return col_out;
}

//BOOST_AUTO_TEST_SUITE(davidson_test)

BOOST_AUTO_TEST_CASE(davidson_full_matrix) {

    int size = 1000;
    int neigen = 10;
    double eps = 0.01;
    Eigen::MatrixXd A = init_matrix(size,eps,false);

    DavidsonSolver DS;
    DS.solve(A,neigen);
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A);

    auto lambda = DS.eigenvalues();
    auto lambda_ref = es.eigenvalues().head(neigen);
    bool check_eigenvalues = lambda.isApprox(lambda_ref,1E-6);

    BOOST_CHECK_EQUAL(check_eigenvalues,1);

}

BOOST_AUTO_TEST_CASE(olsen_full_matrix) {

    int size = 1000;
    int neigen = 10;
    double eps = 0.01;
    Eigen::MatrixXd A = init_matrix(size,eps,false);

    DavidsonSolver DS;
    DS.set_correction("OLSEN");
    DS.solve(A,neigen);

    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A);

    auto lambda = DS.eigenvalues();
    auto lambda_ref = es.eigenvalues().head(neigen);
    bool check_eigenvalues = lambda.isApprox(lambda_ref,1E-6);

    BOOST_CHECK_EQUAL(check_eigenvalues,1);

}

BOOST_AUTO_TEST_CASE(jacobi_full_matrix) {

    int size = 50;
    int neigen = 2;
    double eps = 0.01;
    Eigen::MatrixXd A = init_matrix(size,eps,false);

    DavidsonSolver DS;
    DS.set_correction("JACOBI");
    DS.set_jacobi_linsolve("CG");
    DS.solve(A,neigen);
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A);

    auto lambda = DS.eigenvalues();
    auto lambda_ref = es.eigenvalues().head(neigen);
    bool check_eigenvalues = lambda.isApprox(lambda_ref,1E-6);

    BOOST_CHECK_EQUAL(check_eigenvalues,1);

}

BOOST_AUTO_TEST_CASE(davidson_matrix_free) {

    int size = 1000;
    int neigen = 10;

    TestOperator Aop(size);
    DavidsonSolver DS;
    DS.solve(Aop,neigen);

    Eigen::MatrixXd A = Aop.get_full_mat();
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A);

    auto lambda = DS.eigenvalues();
    auto lambda_ref = es.eigenvalues().head(neigen);
    bool check_eigenvalues = lambda.isApprox(lambda_ref,1E-6);

    BOOST_CHECK_EQUAL(check_eigenvalues,1);

}

BOOST_AUTO_TEST_CASE(olsen_matrix_free) {

    int size = 1000;
    int neigen = 10;

    TestOperator Aop(size);
    DavidsonSolver DS;
    DS.set_correction("OLSEN");
    DS.solve(Aop,neigen);

    Eigen::MatrixXd A = Aop.get_full_mat();
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A);

    auto lambda = DS.eigenvalues();
    auto lambda_ref = es.eigenvalues().head(neigen);
    bool check_eigenvalues = lambda.isApprox(lambda_ref,1E-6);

    BOOST_CHECK_EQUAL(check_eigenvalues,1);

}

BOOST_AUTO_TEST_CASE(jacobi_matrix_free) {

    int size = 50;
    int neigen = 2;

    TestOperator Aop(size);
    DavidsonSolver DS;
    DS.set_correction("JACOBI");
    DS.set_jacobi_linsolve("CG");
    DS.solve(Aop,neigen);

    Eigen::MatrixXd A = Aop.get_full_mat();
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(A);

    auto lambda = DS.eigenvalues();
    auto lambda_ref = es.eigenvalues().head(neigen);
    bool check_eigenvalues = lambda.isApprox(lambda_ref,1E-6);

    BOOST_CHECK_EQUAL(check_eigenvalues,1);

}

//BOOST_AUTO_TEST_SUITE_END()


#ifndef FUNCTIONS_HPP
#define FUNCTIONS_HPP

#include <iostream>
#include <vector>
#define _USE_MATH_DEFINES
#include <math.h>
#include <array>
#include <random>

#include <Eigen/Dense>
#include "particle.hpp"
#include "constants.hpp"

using namespace Eigen;

MatrixXd CalcInput(const float &time);

std::array<MatrixXd,2> Observation(MatrixXd &x_true, MatrixXd &xd, const MatrixXd &u, const MatrixXd &RFID);

MatrixXd MotionModel(const MatrixXd &x, const MatrixXd &u);

double Pi2Pi(const double &angle);

#endif // FUNCTIONS_HPP


#include "apch.h"
#include "Maths.h"
	//#include <Eigen/Core>
	//#include <Eigen/Geometry>
	//#include <Eigen/Dense>

namespace A {

	Eigen::Affine3f CreateOrthographicProjection(float left, float right, float bottom, float top, float z_near, float z_far)
	{
		AP_PROFILE_FN();
		Eigen::Affine3f proj = Eigen::Affine3f::Identity();
		proj(0, 0) = 2 / (right - left);
		proj(1, 1) = 2 / (top - bottom);
		proj(2, 2) = 2 / (z_near - z_far);
		proj(0, 3) = (right + left) / (left - right);
		proj(1, 3) = (top + bottom) / (bottom - top);
		proj(2, 3) = (z_far + z_near) / (z_near - z_far);
		proj(3, 3) = 1;
		return proj;
	}

}


#include <Eigen/Eigenvalues>
#include <eigen3/unsupported/Eigen/MPRealSupport>
#include <mpfr.h>
#include <iostream>

typedef mpfr::mpreal realt;
typedef std::complex<realt> complext;
typedef realt __plant_typet;
#define CONTROL_TYPES_H_
#define interval(x) x
#include "benchmark.h"

using namespace Eigen;
using namespace std;
using namespace mpfr;

#define EXIT_INCREASE_K 2
#define EXIT_INCREASE_SAMPLE_RATE 3
#define PRECISION 256
#define NUM_PROG_ARGS 2 + NSTATES
#define K_SIZE_ARG_INDEX 1u
#define K_ARG_OFFSET 2u

typedef Matrix<realt, NSTATES, NSTATES> matrixt;

//const realt _controller_K[] = { 0.0234375,-0.1328125, 0.00390625 };
//const realt _controller_K[] = { 0.0234375,0.1328125, 0.00390625 };
realt _controller_K[NSTATES];

bool is_imaginary(const realt &imaginary_offset, const complext &complex) {
  const realt imag_value = std::imag(complex);
  cout << "imag_value: " << imag_value << endl;
  cout << "imaginary_offset: " << imaginary_offset << endl;
  return abs(imag_value) > imaginary_offset;
}

int main(const int argc, const char * const argv[]) {
  const realt two = "2.0";
  const realt two_pi = const_pi() * two;
  const realt imaginary_offset = pow(two, -PRECISION/4);
  if (argc != NUM_PROG_ARGS) return EXIT_FAILURE;
  mpreal::set_default_prec(PRECISION);
  const realt K_SIZE = argv[K_SIZE_ARG_INDEX];
  for (size_t i=0; i < NSTATES; ++i)
    _controller_K[i]=argv[i + K_ARG_OFFSET];

  Matrix<realt, NSTATES, NSTATES> A;
  for (size_t i = 0; i < NSTATES; ++i) {
    for (size_t j = 0; j < NSTATES; ++j) {
      A(i, j) = _controller_A[i][j];
    }
  }
  Matrix<realt, NSTATES, 1> B;
  for (size_t i = 0; i < NSTATES; ++i) {
    B(i) = _controller_B[i];
  }
  Matrix<realt, 1, NSTATES> K;
  for (size_t i = 0; i < NSTATES; ++i) {
    K[i] = _controller_K[i];
  }

  // Check K_SIZE
  matrixt result = A - B * K;
  EigenSolver<matrixt> eigenSpace(result);
  if (Success != eigenSpace.info())
    return EXIT_FAILURE;
  const EigenSolver<matrixt>::EigenvalueType eigenvalues = eigenSpace.eigenvalues();
  cout << "num_eigenvalues: " << eigenvalues.size() << endl;
  for (size_t i=0; i < eigenvalues.size(); ++i) {
    cout << "eigenvalue: " << eigenvalues[i] << endl;
    if (!is_imaginary(imaginary_offset, eigenvalues[i])) continue;
    const realt angle = std::arg(eigenvalues[i]);
    const realt expected = abs(two_pi / angle);
    cout << "expected_k_size=" << expected << endl;
    cout << "actual_k_size=" << K_SIZE << endl;
    if (expected > K_SIZE)
      return EXIT_INCREASE_K;
  }

  // Check sample rate
  /*Matrix<realt, Dynamic, Dynamic> vertices(Matrix<realt, Dynamic, Dynamic>::Ones(NSTATES, ::pow(2, NSTATES)));
  int step = 1;
  for (size_t row = 0; row < vertices.rows(); ++row) {
    for (size_t col = 1; col < vertices.cols(); ++col) {
      vertices.coeffRef(row, col) = vertices.coeffRef(row, col - 1);
      if (col % step == 0) vertices.coeffRef(row, col) = -vertices.coeffRef(row, col);
    }
    step <<= 1;
  }
  const Matrix<complext, NSTATES, NSTATES> eigenvectors(eigenSpace.eigenvectors());
  cout << "eigenvectors: " << endl << eigenvectors << endl;
  matrixt pseudo_eigenvectors(eigenvectors.real());
  for (size_t i=0; i < NSTATES - 1; ++i) {
    if (!is_imaginary(imaginary_offset, eigenvalues[i])) continue;
    pseudo_eigenvectors.col(i + 1) = eigenvectors.col(i).imag();
    ++i;
  }
  cout << "pseudo_eigenvectors: " << endl << pseudo_eigenvectors << endl;
  vertices *= pseudo_eigenvectors.transpose();*/

  return EXIT_SUCCESS;
}


/*
 * File:   FCLayer.hpp
 * Author: heshan
 *
 * Created on June 7, 2018, 8:46 PM
 */

#ifndef FCLAYER_HPP
#define FCLAYER_HPP

#include <iostream>
#include <Eigen>
#include "Activation.hpp"

class FCLayer {
public:
    FCLayer();
    /**
     * Constructor
     *
     * @param dimensions: dimensions of the input matrix (depth, height, width)
     * @param outputs: no of outputs
     */
    FCLayer(std::tuple<int, int, int> dimensions, int outputs);
    /**
     *
     * @param orig
     */
    FCLayer(const FCLayer& orig);
    /**
     *
     */
    virtual ~FCLayer();
    /**
     * Initialize weight matrix and bias values
     *
     * @return 0
     */
    int initMat();
    /**
     *
     * @param input: input matrix
     * @return
     */
    Eigen::MatrixXd * forward(Eigen::MatrixXd * input);
    /**
     * Return the dimension of the layer output
     *
     * @return a tuple
     */
    std::tuple<int, int, int> getOutputDims();
private:
public:
    int height, width, depth,outputs;
    Eigen::MatrixXd ** weights;
    Eigen::MatrixXd bias;
    Eigen::MatrixXd * output; // outputs before activation
    Eigen::MatrixXd * activatedOut;

};

#endif /* FCLAYER_HPP */


// inverse_gamma_distribution_example.cpp

// Copyright Paul A. Bristow 2010.
// Copyright Thomas Mang 2010.

// Use, modification and distribution are subject to the
// Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

// Example 1 of using inverse gamma
#include <boost/math/distributions/inverse_gamma.hpp>
using boost::math::inverse_gamma_distribution;  //  inverse_gamma_distribution.
using boost::math::inverse_gamma;

#include <boost/math/special_functions/gamma.hpp>
using boost::math::tgamma; // Used for naive pdf as a comparison.

#include <boost/math/distributions/gamma.hpp>
using boost::math::inverse_gamma_distribution;

#include <iostream>
using std::cout;    using std::endl;
#include <iomanip>
using std::setprecision;
#include <cmath>
using std::sqrt;

int main()
{

  cout << "Example using Inverse Gamma distribution. " << endl;
  // TODO - awaiting a real example using Bayesian statistics.

#ifdef BOOST_NO_NUMERIC_LIMITS_LOWEST
  int max_digits10 = 2 + (boost::math::policies::digits<double, boost::math::policies::policy<> >() * 30103UL) / 100000UL;
  cout << "BOOST_NO_NUMERIC_LIMITS_LOWEST is defined" << endl;
#else
  int max_digits10 = std::numeric_limits<double>::max_digits10;
#endif
  cout << "Show all potentially significant decimal digits std::numeric_limits<double>::max_digits10 = "
    << max_digits10 << endl;
  cout.precision(max_digits10); //

  double shape = 1.;
  double scale = 1.;
  double x = 0.5;
  // Construction using default RealType double, and default shape and scale..
  inverse_gamma_distribution<> my_inverse_gamma(shape, scale); // (alpha, beta)

  cout << "my_inverse_gamma.shape() = " << my_inverse_gamma.shape()
    << ", scale = "<< my_inverse_gamma.scale() << endl;
  cout << "x = " << x << ", pdf = " << pdf(my_inverse_gamma, x)
    << ", cdf = " << cdf(my_inverse_gamma, x) << endl;

  // Construct using  typedef and default shape and scale parameters.
  inverse_gamma my_ig;

  inverse_gamma my_ig23(2, 3);
  cout << "my_inverse_gamma.shape() = " << my_ig23.shape()
    << ", scale = "<< my_ig23.scale() << endl;
  cout << "x = " << x << ", pdf = " << pdf(my_ig23, x)
    << ", cdf = " << cdf(my_ig23, x) << endl;

  // Example of providing an 'out of domain' or 'bad' parameter,
  // here a shape < 1, for which mean is not defined.
  // Try block is essential to catch the exception message.
  // (Uses the default policy which is to throw on all errors).
  try
  {
    inverse_gamma if051(0.5, 1);
    //inverse_gamma if051(0.5, 1);
    cout << "mean(if051) = " << mean(if051) << endl;
  }
  catch(const std::exception& e)
  { // Always useful to include try & catch blocks because default policies
    // are to throw exceptions on arguments that cause errors like underflow, overflow.
    // Lacking try & catch blocks, the program will abort without a message below,
    // which may give some helpful clues as to the cause of the exception.
    std::cout <<
      "\n""Message from thrown exception was:\n   " << e.what() << std::endl;
  }

  return 0;
}  // int main()

/*

Output is:
  Example using Inverse Gamma distribution.
  std::numeric_limits<double>::max_digits10 = 17
  my_inverse_gamma.shape() = 1, scale = 1
  x = 0.5, pdf = 0.54134113294645081, cdf = 0.1353352832366127
  my_inverse_gamma.shape() = 2, scale = 3
  x = 0.5, pdf = 0.17847015671997774, cdf = 0.017351265236664509

  Message from thrown exception was:
     Error in function boost::math::mean(const inverse_gamma_distribution<double>&): Shape parameter is 0.5, but for a defined mean it must be > 1

*/


#include <vector> /* representing state */
#include <algorithm> /* for numeric min/max */
#include <cmath> /* log, exp, fmod */

#include <boost/numeric/odeint.hpp>
#include <ios>
using namespace boost::numeric::odeint;

typedef double icing_float;

// [1] - Reference Paper: TBME-01160-2016
// [2] - Reference Paper: computer methods and programs in biomedicine 102 (2011) 192–205
template<typename U = icing_float>
class ChaseIcing {
    // Each enum maps to an index within the state vector for accessing that
    // function's value.
    enum Function { fn_G = 0
        , fn_Q
        , fn_I
        , fn_P1
        , fn_P2
        , fn_P
        , fn_u_en
    };
    using _state_type = std::vector<U>;

    /* a bunch of constants that should be in the namespace */
    const U n_I = 0.0075; // 1/min, Table II, [1]
    const U d1 = -std::log(0.5)/20.0; // 1/min, Table II, [1]
    const U d2 = -std::log(0.5)/100.0; // 1/min, Table II, [1]
    const U P_max = 6.11; // mmol/min, Table II, [1]

    auto P_min(const _state_type &x)
    {
        return std::min(d2 * x[fn_P2], P_max);
    }

    auto alpha_decay(const U variable, const U decay_parameter)
    {
        return variable / (1.0 + decay_parameter * variable);
    }

    auto Q_frac(const _state_type &x)
    {
        const U a_G = 1.0 / 65.0; // Table II, [1]
        const U Q = x[fn_Q];
        return alpha_decay(Q, a_G);
    }

    // PN(t) -> Parenteral nutrition input, eg IV dextrose
    auto _P(const _state_type &x)
    {
        const U _PN_ext = dextrose_rate; // TODO -> derive this over the network

        return P_min(x) + _PN_ext;
    }

    auto _u_en(const _state_type &x)
    {
        const U k1 = 14.9; // mU * l / mmol/min, Table II, [1]
        const U k2 = -49.9; // mU/min, Table II, [1]
        const U u_min = 16.7; // mU/min, Table II, [1]
        const U u_max = 266.7; // mU/min, Table II, [1]
        const U G = x[fn_G];
        return std::min(std::max(u_min, k1 * G + k2), u_max);
    }

    auto G_dot(const _state_type &x)
    {
        const U p_G = 0.006; // End of section 4.1, in [2]
        const U S_I = 0.5e-3; // TODO: patient specific
        const U G = x[fn_G];
        const U Q = x[fn_Q];
        const U P = _P(x);
        const U EGP = 1.16; // mmol/min Table II, [1]
        const U CNS = 0.3; // mmol/min Table II, [1]
        const U V_G = 13.3; // L, Table II, [1]

        U dGdt = 0.0;
        // G' = -p_G G(t)
        //	 - S_I G(t) (Q(t) / (1 + a_G Q(t)))
        //	 + (P(t) + EGP -CNS)/V_G
        dGdt += -p_G * G;
        dGdt += -S_I*G * Q_frac(x);
        dGdt += (P + EGP - CNS)/V_G;

        return dGdt;
    }

    auto Q_dot(const _state_type &x)
    {
        const U I = x[fn_I];
        const U Q = x[fn_Q];
        const U n_C = 0.0075; // 1/min, Table II, [1]

        return n_I * (I - Q) - n_C * Q_frac(x);
    }

    auto I_dot(const _state_type &x)
    {
        const U n_K = 0.0542; // 1/min, Table II, [1]
        const U n_L = 0.1578; // 1/min, Table II, [1]
        const U a_I = 1.7e-3; // 1/mU, Table II, [1]
        const U V_I = 4.0; // L, Table II, [1]
        const U x_L = 0.67; // unitless, Table II, [1]

        const U u_ex = insulin_rate; // TODO: get this over the network

        const U Q = x[fn_Q];
        const U I = x[fn_I];
        const U u_en = _u_en(x);

        auto dIdt = U(0.0);
        // I' = - n_K I(t)
        //	- n_L (I(t)/(1+a_I I(t)))
        //	- n_I (I(t) - Q(t))
        //	+ u_ex(t) / V_I
        //	+ (1 - x_L) u_en / V_I
        dIdt += -n_K * I;
        dIdt += -n_L * alpha_decay(I, a_I);
        dIdt += -n_I * (I - Q);
        dIdt += u_ex / V_I;
        dIdt += (1.0 - x_L) * u_en / V_I;
        return dIdt;
    }

    auto P1_dot(const _state_type &x)
    {
        const auto D = U(0.0); // enteral feed rate TODO: get from network
        return -d1 * x[fn_P1] + D;
    }

    auto P2_dot(const _state_type &x)
    {
        return -P_min(x) + d1 * x[fn_P1];
    }

    void copy(const ChaseIcing &other)
    {
        data = other.data;
        insulin_rate = other.insulin_rate;
        dextrose_rate = other.dextrose_rate;
    }

    /* data is the container which contains the most up to date representation of the model's state
     *
     * Should only be accessed using the enum Function data type
     */
    _state_type data;
public:
    using state_type = _state_type;
    void operator() (const state_type &x, state_type &dxdt, const U t)
    {
        dxdt[fn_G] = G_dot(x);
        dxdt[fn_Q] = Q_dot(x);
        dxdt[fn_I] = I_dot(x);
        dxdt[fn_P1] = P1_dot(x);
        dxdt[fn_P2] = P2_dot(x);
    }

    auto glucose()
    {
        return data[fn_G];
    }

    auto q()
    {
        return data[fn_Q];
    }

    auto i()
    {
        return data[fn_I];
    }

    auto p1()
    {
        return data[fn_P1];
    }

    auto p2()
    {
        return data[fn_P2];
    }

    int run(U time_start
            , U time_end
            , U dt
            , U insulin_rate_mUpermin
            , U dextrose_rate_mmolpermin)
    {

        insulin_rate = insulin_rate_mUpermin;
        dextrose_rate = dextrose_rate_mmolpermin;

        /* model doesnt modify inplace, so we make copies of its data
         * for integration */
        auto x = data;
        auto step = stepper;

        integrate_const(step, *this, x, time_start, time_end, dt);

        data = x;
        stepper = step;

        return 0;
    }

    /* run the model until time_end using default rates and time step
     */
    int run(U time_end)
    {
        return run(0.0, time_end, 0.1, insulin_rate, dextrose_rate);
    }

    /* run the model with implicitly provided rates
     */
    int run(U time_start, U time_end, U dt)
    {
        return run(time_start, time_end, dt, insulin_rate, dextrose_rate);
    }

    /* insulin_rate is the exogenous IV insulin rate, in mU/min (not mmol/min as listed in [1])
     *
     * Available for public modification, but use run() for most purposes.
     */
    U insulin_rate;

    /* dextrose_rate is the exogenous IV dextrose rate, in mmol/min
     *
     * Available for public modification, but use run() for most purposes.
     */
    U dextrose_rate;

    /* stepper is the Boost integration type, we use RK4 but you may use any you like
     */
    runge_kutta4<state_type> stepper;

    /* public constructor
     */
    ChaseIcing(state_type _data) : data(_data) {};

    /* copy constructor
     */
    ChaseIcing(const ChaseIcing &other)
    {
        copy(other);
    }

    /* assignment operator
     */
    ChaseIcing& operator=(const ChaseIcing &other)
    {
        copy(other);
        return *this;
    }
};


#include "../voxelizer.h"
#include "../volume.h"
#include "../traversal.h"

#include <gp_Pln.hxx>
#include <Bnd_Box.hxx>
#include <BRepBndLib.hxx>
#include <BRepBuilderAPI_MakeFace.hxx>
#include <BRepMesh_IncrementalMesh.hxx>

#include <Eigen/Dense>

#include <gtest/gtest.h>

TEST(NormalEstimation, EigenPCA) {
	static Eigen::Vector3f Z(0, 0, 1);
	Eigen::MatrixXf points(10, 3);
	points.setRandom();
	points.col(2).setZero();

	Eigen::MatrixXf centered = points.rowwise() - points.colwise().mean();
	Eigen::MatrixXf cov = centered.adjoint() * centered;
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eig(cov);

	ASSERT_FLOAT_EQ(std::abs(eig.eigenvectors().col(0).dot(Z)), 1.);
}

TEST(NormalEstimation, OCC) {
	gp_Pln p(1, 2, 3, 4);
	auto face = BRepBuilderAPI_MakeFace(p, -1, 1, -1, 1).Face();
	BRepMesh_IncrementalMesh(face, 0.1);

	auto surf = BRep_Tool::Surface(face);
	auto dir = Handle_Geom_Plane::DownCast(surf)->Position().Direction();

	Eigen::Vector3f norm(dir.X(), dir.Y(), dir.Z());
	std::cout << "Face normal " << norm << std::endl;

	Bnd_Box B;
	BRepBndLib::AddClose(face, B);

	double d = 0.01;
	double x0, y0, z0, x1, y1, z1;
	B.Get(x0, y0, z0, x1, y1, z1);

	auto storage = new chunked_voxel_storage<bit_t>(
		x0 - d,
		y0 - d,
		z0 - d, d,
		(x1 - x0) / d + 2,
		(y1 - y0) / d + 2,
		(z1 - z0) / d + 2,
		64);
	auto vox = voxelizer(face, storage);
	vox.Convert();

	std::cout << "count " << storage->count() << std::endl;

	auto it = storage->begin();
	std::advance(it, storage->count() / 2);

	std::cout << "center " << (*it).format() << std::endl;

	int i = 2;

	visitor<26> vis;
	vis.max_depth = 0.1 * i / d;

	std::cout << "md " << (0.1 * i) << std::endl;

	std::vector<float> coords;

	auto selection = storage->empty_copy();

	vis([&coords, &selection](const tagged_index& pos) {
		if (pos.which == tagged_index::VOXEL) {
			coords.push_back(pos.pos.get(0));
			coords.push_back(pos.pos.get(1));
			coords.push_back(pos.pos.get(2));
			selection->Set(pos.pos);
		}
		else {
			throw std::runtime_error("Unexpected");
		}
	}, storage, *it);

	/*{
		std::ofstream ofs("m2.obj");
		((chunked_voxel_storage<bit_t>*)selection)->obj_export(ofs);
	}

	storage->boolean_subtraction_inplace(selection);

	{
		std::ofstream ofs("m1.obj");
		storage->obj_export(ofs);
	}*/

	std::cout << "neighbours " << (coords.size() / 3) << std::endl;

	Eigen::MatrixXf points = Eigen::Map<Eigen::MatrixXf>(coords.data(), 3, coords.size() / 3).transpose();

	Eigen::MatrixXf centered = points.rowwise() - points.colwise().mean();
	Eigen::MatrixXf cov = centered.adjoint() * centered;
	Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> eig(cov);

	auto estimated = eig.eigenvectors().col(0);

	std::cout << estimated << std::endl;

	auto angle = std::acos(estimated.dot(norm));
	if (angle > M_PI / 2) {
		angle = M_PI - angle;
	}
	auto angle_degrees = angle / M_PI * 180.;

	std::cout << "angle " << angle_degrees << "d" << std::endl;

	ASSERT_LT(angle_degrees, 1.);
}


// Boost.Geometry (aka GGL, Generic Geometry Library)
// QuickBook Example

// Copyright (c) 2011 Barend Gehrels, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

//[register_ring
//` Show the use of the macro BOOST_GEOMETRY_REGISTER_RING

#include <iostream>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/register/ring.hpp>

typedef boost::geometry::model::d2::point_xy<double> point_2d;

BOOST_GEOMETRY_REGISTER_RING(std::vector<point_2d>) /*< The magic: adapt vector to Boost.Geometry Ring Concept >*/

int main()
{
    // Normal usage of std::
    std::vector<point_2d> ring;
    ring.push_back(point_2d(1, 1));
    ring.push_back(point_2d(2, 2));
    ring.push_back(point_2d(2, 1));

    // Usage of Boost.Geometry
    boost::geometry::correct(ring);
    std::cout << "Area: "  << boost::geometry::area(ring) << std::endl;
    std::cout << "WKT: "  << boost::geometry::wkt(ring) << std::endl;

    return 0;
}

//]

//[register_ring_output
/*`
Output:
[pre
Area: 0.5
WKT: POLYGON((1 1,2 2,2 1,1 1))
]
*/
//]


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_ULPDIST_HPP_INCLUDED
#define BOOST_SIMD_IEEE_FUNCTIONS_SCALAR_ULPDIST_HPP_INCLUDED
#include <boost/simd/ieee/functions/ulpdist.hpp>
#include <boost/simd/include/constants/eps.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/functions/scalar/abs.hpp>
#include <boost/simd/include/functions/scalar/tofloat.hpp>
#include <boost/simd/include/functions/scalar/ldexp.hpp>
#include <boost/simd/include/functions/scalar/frexp.hpp>
#include <boost/simd/include/functions/scalar/max.hpp>
#include <boost/simd/include/functions/scalar/dist.hpp>
#include <boost/simd/include/functions/scalar/subs.hpp>
#include <boost/simd/include/functions/scalar/is_nan.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( ulpdist_, tag::cpu_
                                    , (A0)
                                    , (scalar_< int_<A0> >)
                                      (scalar_< int_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return (a0>a1) ? subs(a0,a1) : subs(a1,a0);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( ulpdist_, tag::cpu_
                                    , (A0)
                                    , (scalar_< uint_<A0> >)
                                      (scalar_< uint_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_FORCEINLINE BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      return dist(a0,a1);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( ulpdist_, tag::cpu_
                                    , (A0)
                                    , (scalar_< floating_<A0> >)
                                      (scalar_< floating_<A0> >)
                                    )
  {
    typedef A0 result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename dispatch::meta::as_integer<A0>::type itype;

      if (a0 == a1)               return Zero<A0>();
      if (is_nan(a0)&&is_nan(a1)) return Zero<A0>();

      itype e1, e2;

      A0 m1 = boost::simd::frexp(a0, e1);
      A0 m2 = boost::simd::frexp(a1, e2);

      itype expo = -boost::simd::max(e1, e2);

      A0 e = (e1 == e2) ? boost::simd::abs(m1-m2)
                            :   boost::simd::abs( boost::simd::ldexp(a0, expo)
                                                - boost::simd::ldexp(a1, expo)
                                                );
      return e/Eps<A0>();
    }
  };
} } }

#endif


/**
 * Copyright (c) 2022 <Daumantas Kavolis>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#pragma once

#include "config.hpp"

MSVC_WARNING_DISABLE(4619)
#include <boost/container/small_vector.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/adaptor/transformed.hpp>
#include <boost/range/size.hpp>
MSVC_WARNING_POP()

#include "polynomial.hpp"
#include "sequence.hpp"
#include "traits.hpp"

namespace poly {
template <class Poly, std::size_t N = SmallStorageSize>
class PolynomialSeries {
 public:
  using Traits = typename Poly::Traits;
  using Real = typename Traits::Real;
  using OrderType = typename Traits::OrderType;

  template <typename T>
  using small_vector = boost::container::small_vector<T, N>;

  explicit PolynomialSeries(OrderType n) { resize(n); }

  template <class Range, typename = std::enable_if_t<detail::is_range<Range>::value>>
  explicit PolynomialSeries(Range const& coefficients) {
    auto count = boost::size(coefficients);
    coefficients_.resize(count);
    polynomials_.resize(count);
    for (auto&& [index, coefficient] : coefficients | boost::adaptors::indexed()) {
      polynomials_[index].order(narrow_cast<OrderType>(index));
      coefficients_[index] = static_cast<Real>(coefficient);
    }
  }

  auto coefficients() noexcept -> view<Real> {
    return {coefficients_.data(), coefficients_.size()};
  }
  [[nodiscard]] auto coefficients() const noexcept -> view<Real const> {
    return {coefficients_.data(), coefficients_.size()};
  }
  [[nodiscard]] auto polynomials() const noexcept -> view<Poly const> {
    return {polynomials_.data(), polynomials_.size()};
  }

  auto operator[](std::size_t index) noexcept -> Real& { return coefficients_[index]; }
  [[nodiscard]] auto operator[](std::size_t index) const noexcept -> Real {
    return coefficients_[index];
  }

  auto at(std::size_t index) noexcept -> Real& { return coefficients_.at(index); }
  [[nodiscard]] auto at(std::size_t index) const noexcept -> Real {
    return coefficients_.at(index);
  }

  template <class F, typename = std::enable_if_t<std::is_invocable_r_v<Real, F, Real>>>
  [[nodiscard]] static auto project(F const& function, OrderType order) -> PolynomialSeries {
    Poly poly{order};
    view<Real const> abscissa = poly.abscissa();
    return project(abscissa | boost::adaptors::transformed(
                                  [&function](Real const& x) { return function(x); }));
  }

  template <class Range, typename = std::enable_if_t<detail::is_range<Range>::value>>
  [[nodiscard]] static auto project(Range const& y_range) -> PolynomialSeries {
    OrderType count = narrow<OrderType>(boost::size(y_range));
    if (count == 0) return PolynomialSeries(0);
    OrderType max_order = count - 1;

    Poly poly{max_order};
    view<Real const> weights = poly.weights();
    view<Real const> abscissa = poly.abscissa();

    PolynomialSeries series(max_order);

    for (auto&& [j, y] : y_range | boost::adaptors::indexed()) {
      for (auto&& [i, f] :
           polynomial_sequence<Poly>(count, abscissa[j]) | boost::adaptors::indexed()) {
        series.coefficients_[i] += f * weights[j] * y;
      }
    }

    return series;
  }

  [[nodiscard]] auto operator()(Real x) const -> Real {
    if constexpr (Traits::has_next) {
      if (size() == 0) return 0;
      Real t0 = polynomials_[0](x);
      Real f = coefficients_[0] * t0;
      if (size() == 1) return f;
      Real t1 = polynomials_[1](x);
      for (std::size_t i = 1; i < size(); i++) {
        f += coefficients_[i] * t1;
        std::swap(t0, t1);
        t1 = polynomials_[i].next(x, t0, t1);
      }
      return f;
    } else {
      Real f = 0;
      for (auto&& [index, polynomial] : polynomials_ | boost::adaptors::indexed())
        f += coefficients_[index] * polynomial(x);
      return f;
    }
  }

  [[nodiscard]] auto size() const noexcept -> std::size_t { return polynomials_.size(); }
  void resize(OrderType new_size) {
    OrderType old_size = narrow<OrderType>(size());
    polynomials_.resize(new_size);
    coefficients_.resize(new_size, 0);

    for (OrderType i = old_size; i < new_size; i++) polynomials_[i].order(i);
  }

  template <bool check = false>
  [[nodiscard]] auto weights(bounds_check<check> c = no_bounds_check) const -> view<Real const> {
    return polynomials_[size() - 1].weights(c);
  }
  template <bool check = false>
  [[nodiscard]] auto abscissa(bounds_check<check> c = no_bounds_check) const -> view<Real const> {
    return polynomials_[size() - 1].abscissa(c);
  }

  static auto domain() noexcept -> std::pair<Real, Real> { return Poly::domain(); }

 private:
  small_vector<Real> coefficients_;
  small_vector<Poly> polynomials_;
};
}  // namespace poly


#pragma once

#include "Common.hh"

#include <ACG/Math/VectorT.hh>

#include <Eigen/Dense>

namespace betri
{

class Fitting
{
public:

	// list of available solvers
	enum Solver
	{
		normal_equation,
		qr_decomposition,
		adaptive
	};

	using EigenMatT = Eigen::MatrixXd;
	using EigenVectorT = Eigen::VectorXd;

	using Vertices = std::vector<VertexHandle>;

	explicit Fitting(BezierTMesh &mesh) :
		m_mesh(mesh), m_degree(1), m_solver(Solver::normal_equation) {}

	void degree(size_t degree) { m_degree = degree; }
	size_t degree() const { return m_degree; }

	void solver(Solver solver) { m_solver = solver; }
	Solver solver() const { return m_solver; }

	virtual bool solve() = 0;

	virtual void prepare() = 0;
	virtual void cleanup() = 0;

protected:

	static Scalar calcCoeffs(Vec2 uv, int i, int j, size_t degree)
	{
		assert(std::islessequal(uv[0], 1.0));
		assert(std::islessequal(uv[1], 1.0));
		return eval(i, j, uv, degree);
	}

	static bool solveSystem(
		EigenMatT &A,
		EigenVectorT &rhs,
		EigenVectorT &result,
		Solver solver=normal_equation
	) {
		Eigen::ComputationInfo info;

		if (solver == adaptive) {
			solver = std::max(A.rows(), A.cols()) > 100 ? qr_decomposition : normal_equation;
		}

		// solve system with specified solver
		switch (solver) {
			case normal_equation:
			{
				auto solver = (A.transpose() * A).ldlt();
				result = solver.solve(A.transpose() * rhs);
				info = solver.info();
				break;
			}
			case qr_decomposition:
			{
				auto solver = A.colPivHouseholderQr();
				result = solver.solve(rhs);
				info = solver.info();
				break;
			}
		}

		if (info != Eigen::Success) {
			std::cerr << __FUNCTION__ << ": solver failed! (" << info << ")\n";
			return false;
		}

		return true;
	}

	///////////////////////////////////////////////////////////
	// member variables
	///////////////////////////////////////////////////////////

	BezierTMesh &m_mesh;
	size_t m_degree;
	Solver m_solver;
};

} // namespace betri


#include <iostream>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

int main()
{
  Eigen::MatrixXf m(2,4);
  Eigen::VectorXf v(2);

  m << 1, 23, 6, 9,
       3, 11, 7, 2;

  v << 2,
       3;

  MatrixXf::Index index;
  // find nearest neighbour
  (m.colwise() - v).colwise().squaredNorm().minCoeff(&index);

  cout << "Nearest neighbour is column " << index << ":" << endl;
  cout << m.col(index) << endl;
}


// system includes -----------------------------------------------
#include <boost/math/constants/constants.hpp>
#include <boost/program_options.hpp>
#include <iostream>
// own includes --------------------------------------------------
#include "base/eigen2hdf.hpp"
#include "base/init.hpp"
#include "base/timer.hpp"
#include "fft/fft2.hpp"
#include "ridgelet/rc_linearize.hpp"
#include "ridgelet/ridgelet_cell_array.hpp"
#include "ridgelet/ridgelet_frame.hpp"
#include "ridgelet/rt.hpp"

using namespace std;

const char* fname = "test_rt_truncate.h5";

typedef FFT fft_t;
typedef RT<std::complex<double>, RidgeletFrame, fft_t> RT_t;
typedef RT_t::array_t array_t;
typedef RT_t::complex_array_t complex_array_t;
typedef RT_t::rt_coeff_t rt_coeff_t;

void dump_frc(const std::vector<rt_coeff_t>& f_rc, const RidgeletFrame& rt)
{
  const char* fname = "f_rc.h5";
  hid_t file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
  for (unsigned int i = 0; i < f_rc.size(); ++i) {
    stringstream ss;
    ss << rt.lambdas()[i];
    string slam = ss.str();
    eigen2hdf::save(file, slam, f_rc[i]);
  }
  H5Fclose(file);
  cout << "Written f(lambda, t) to " << fname << "\n";
}

int main(int argc, char* argv[])
{
  SOURCE_INFO();

  namespace po = boost::program_options;

  unsigned int J, rho_x, rho_y;
  double keep;
  double sigma;

  po::options_description options("options");
  options.add_options()("help", "produce help message")
      ("J,j", po::value<unsigned int>(&J)->default_value(3), "J")
      ("rx,x", po::value<unsigned int>(&rho_x)->default_value(1), "rho_x")
      ("ry,y", po::value<unsigned int>(&rho_y)->default_value(1), "rho_x")
      ("rttre", po::value<double>(&keep)->default_value(1), "percentage of coefficients kept")
      ("sigma", po::value<double>(&sigma)->default_value(1. / 8), "parameter")
      ("save", "save coefficients")
      ("non-smooth", "non-smooth");
  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, options), vm);
  po::notify(vm);

  if (vm.count("help")) {
    std::cout << options << "\n";
    return 0;
  }

  cout << setw(20) << "J: " << J << "\n"
       << setw(20) << "rho_x: " << rho_x << "\n"
       << setw(20) << "rho_y: " << rho_y << "\n"
       << setw(20) << "Nx: " << std::pow(2, J + 2) * rho_x << "\n"
       << setw(20) << "Ny: " << std::pow(2, J + 2) * rho_y << "\n";
  RDTSCTimer timer;

  timer.start();
  RidgeletFrame frame(J, J, rho_x, rho_y);
  double time_frame_constructor = timer.stop();
  timer.print(cout, time_frame_constructor, "RidgeletFrame init");

  const unsigned int ncols = frame.Nx();  // #cols
  const unsigned int nrows = frame.Ny();  // #rows

  RT_t rt(frame);
  typedef typename RT_t::rt_coeff_t rt_coeff_t;
  typedef RidgeletCellArray<rt_coeff_t> rca_t;
  array_t F(nrows / 2, ncols / 2);
  const double sigma2 = sigma * sigma;
  if (vm.count("non-smooth")) {
    Eigen::ArrayXd x = Eigen::ArrayXd::LinSpaced(ncols / 2, 0, 1);
    Eigen::ArrayXd y = Eigen::ArrayXd::LinSpaced(nrows / 2, 0, 1);
    F = x.transpose()
            .replicate(nrows / 2, 1)
            .binaryExpr(y.replicate(1, ncols / 2), [sigma2](double x, double y) {
              return std::exp(-1 / sigma2 * (std::pow(x - 0.5, 2) + std::pow(y - 0.5, 2)));
            });
    F = ((x.transpose().replicate(nrows / 2, 1) - 0.5).cwiseAbs() < sigma)
            .select(F, Eigen::ArrayXXd::Zero(nrows / 2, ncols / 2));

  } else {  // smooth
    Eigen::ArrayXd x = Eigen::ArrayXd::LinSpaced(ncols / 2, 0, 1);
    Eigen::ArrayXd y = Eigen::ArrayXd::LinSpaced(nrows / 2, 0, 1);
    F = x.transpose()
            .replicate(nrows / 2, 1)
            .binaryExpr(y.replicate(1, ncols / 2), [sigma2](double x, double y) {
              return std::exp(-1 / sigma2 * (std::pow(x - 0.5, 2) + std::pow(y - 0.5, 2)));
            });
  }

  fft_t fft;
  // debug
  complex_array_t Fhh(nrows / 2, ncols / 2);
  fft.ft(Fhh, F, false);
  hid_t file;
  if (vm.count("save")) {
    file = H5Fcreate(fname, H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
    eigen2hdf::save(file, "Fhh", Fhh);  // debug
  }
  // ----------------------------------------
  complex_array_t Fh(nrows, ncols);
  Fh.setZero();
  ftcut(Fh, nrows / 2, ncols / 2) = Fhh;
  // hf_zero(Fh); // make sure RT projection is real valued
  rca_t rca(frame);
  timer.start();
  cout << "rt.rt(...)"
       << "\n";
  rt.rt(rca.coeffs(), Fh);

  rca_t rca_copy(frame);
  rca_copy = rca;
  auto& rt_coeffs = rca.coeffs();

  int ncoeffs = 0;
  for (unsigned int i = 0; i < rt_coeffs.size(); ++i) {
    ncoeffs += rt_coeffs[i].rows() * rt_coeffs[i].cols();
  }
  cout << "dim(rt_coeffs): " << ncoeffs << "\n";
  auto time_rt = timer.stop();
  timer.print(cout, time_rt, "rt.rt");

  RCLinearize rcl(frame);

  if (keep < 1) {
    double tre = rcl.get_threshold(rca, keep);
    // apply threshold
    rcl.threshold(rca, tre);
  }

  // -------------------- Inverse transform --------------------
  complex_array_t Fh2(nrows, ncols);
  timer.start();
  cout << "rt.irt(...)"
       << "\n";
  rt.irt(Fh2, rt_coeffs);
  auto time_irt = timer.stop();
  timer.print(cout, time_irt, "rt.irt");

  array_t F2(nrows / 2, ncols / 2);
  complex_array_t Fh2_cut(nrows / 2, ncols / 2);
  Fh2_cut.setZero();
  Fh2_cut = ftcut(Fh2, nrows / 2, ncols / 2);
  // hf_zero(Fh2_cut);
  fft.ift(F2, Fh2_cut);
  auto diff = (F - F2).abs() / (F.rows() * F.cols());
  cout << "(F-F2).abs().sum(): " << diff.sum() << "\n";

  if (vm.count("save")) {
    eigen2hdf::save(file, "Fhl", Fhh);
    eigen2hdf::save(file, "Fh", Fh);
    eigen2hdf::save(file, "R", F);
    eigen2hdf::save(file, "Fh2", Fh2);
    eigen2hdf::save(file, "R2", F2);
    H5Fclose(file);
    cout << "written results to " << fname << "\n";
  }

  return 0;
}


#include <gtest/gtest.h>
#include "../util/util.h"
#include <Eigen/Core>
#include <string>
#include <algorithm>

#ifndef _MSC_VER
extern "C" {
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
#include <csim/update_ops.h>
}
#else
#include <csim/memory_ops.h>
#include <csim/stat_ops.h>
#include <csim/update_ops.h>
#endif

TEST(UpdateTest, SingleQubitGateTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    Eigen::MatrixXcd X(2, 2), Y(2, 2), Z(2, 2), H(2, 2), S(2, 2), T(2, 2), sqrtX(2, 2), sqrtY(2, 2);
    X << 0, 1, 1, 0;
    Y << 0, -1.i, 1.i, 0;
    Z << 1, 0, 0, -1;
    H << 1, 1, 1, -1; H /= sqrt(2.);
    S << 1, 0, 0, 1.i;
    T << 1, 0, 0, (1. + 1.i) / sqrt(2.);
    sqrtX << 0.5 + 0.5i, 0.5 - 0.5i, 0.5 - 0.5i, 0.5 + 0.5i;
    sqrtY << 0.5 + 0.5i, -0.5 - 0.5i, 0.5 + 0.5i, 0.5 + 0.5i;

    UINT target;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];
    typedef std::tuple<std::function<void(UINT, CTYPE*, ITYPE)>, Eigen::MatrixXcd, std::string> testset;
    std::vector<testset> test_list;
    test_list.push_back(std::make_tuple(X_gate, X, "X"));
    test_list.push_back(std::make_tuple(Y_gate, Y, "Y"));
    test_list.push_back(std::make_tuple(Z_gate, Z, "Z"));
    test_list.push_back(std::make_tuple(S_gate, S, "S"));
    test_list.push_back(std::make_tuple(Sdag_gate, S.adjoint(), "Sdag"));
    test_list.push_back(std::make_tuple(T_gate, T, "T"));
    test_list.push_back(std::make_tuple(Tdag_gate, T.adjoint(), "Tdag"));
    test_list.push_back(std::make_tuple(sqrtX_gate, sqrtX, "sqrtX"));
    test_list.push_back(std::make_tuple(sqrtXdag_gate, sqrtX.adjoint(), "sqrtXdag"));
    test_list.push_back(std::make_tuple(sqrtY_gate, sqrtY, "sqrtY"));
    test_list.push_back(std::make_tuple(sqrtYdag_gate, sqrtY.adjoint(), "sqrtYdag"));

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        for (auto tup : test_list) {
            target = rand_int(n);
            auto func = std::get<0>(tup);
            auto mat = std::get<1>(tup);
            auto name = std::get<2>(tup);
            func(target, state, dim);
            test_state = get_expanded_eigen_matrix_with_identity(target, mat, n) * test_state;
            state_equal(state, test_state, dim, name);
        }
    }
    release_quantum_state(state);
}

TEST(UpdateTest, ProjectionAndNormalizeTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;
    const double eps = 1e-14;

    Eigen::MatrixXcd P0(2, 2), P1(2, 2);
    P0 << 1, 0, 0, 0;
    P1 << 0, 0, 0, 1;

    UINT target;
    double prob;

    auto state = allocate_quantum_state(dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        initialize_Haar_random_state(state, dim);
        Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
        for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

        // Z-projection operators
        target = rand_int(n);
        if (rep % 2 == 0) {
            prob = M0_prob(target, state, dim);
            EXPECT_GT(prob, 1e-10);
            P0_gate(target, state, dim);
            ASSERT_NEAR(state_norm(state, dim), prob, eps);
            normalize(prob, state, dim);

            test_state = get_expanded_eigen_matrix_with_identity(target, P0, n)*test_state;
            ASSERT_NEAR(test_state.squaredNorm(), prob, eps);
            test_state.normalize();
            state_equal(state, test_state, dim, "P0 gate");
        }
        else {
            prob = M1_prob(target, state, dim);
            EXPECT_GT(prob, 1e-10);
            P1_gate(target, state, dim);
            ASSERT_NEAR(state_norm(state, dim), prob, eps);
            normalize(prob, state, dim);

            test_state = get_expanded_eigen_matrix_with_identity(target, P1, n)*test_state;
            ASSERT_NEAR(test_state.squaredNorm(), prob, eps);
            test_state.normalize();
            state_equal(state, test_state, dim, "P1 gate");
        }
    }
    release_quantum_state(state);
}

TEST(UpdateTest, SingleQubitRotationGateTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    Eigen::MatrixXcd Identity(2,2),X(2, 2), Y(2, 2), Z(2, 2);
    Identity << 1, 0, 0, 1;
    X << 0, 1, 1, 0;
    Y << 0, -1.i, 1.i, 0;
    Z << 1, 0, 0, -1;

    UINT target;
    double angle;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];
    typedef std::tuple<std::function<void(UINT, double, CTYPE*, ITYPE)>, Eigen::MatrixXcd, std::string> testset;
    std::vector<testset> test_list;
    test_list.push_back(std::make_tuple(RX_gate, X, "Xrot"));
    test_list.push_back(std::make_tuple(RY_gate, Y, "Yrot"));
    test_list.push_back(std::make_tuple(RZ_gate, Z, "Zrot"));

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        for (auto tup : test_list) {
            target = rand_int(n);
            angle = rand_real();
            auto func = std::get<0>(tup);
            auto mat = std::get<1>(tup);
            auto name = std::get<2>(tup);
            func(target, angle, state, dim);
            test_state = get_expanded_eigen_matrix_with_identity(target, cos(angle/2)*Identity + 1.i*sin(angle/2)*mat, n) * test_state;
            state_equal(state, test_state, dim, name);
        }
    }
    release_quantum_state(state);
}

TEST(UpdateTest, TwoQubitGateTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    std::vector<UINT> index_list;
    for (UINT i = 0; i < n; ++i) index_list.push_back(i);

    UINT target,control;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];
    typedef std::tuple<
        std::function<void(UINT, UINT, CTYPE*, ITYPE)>,
        std::function<Eigen::MatrixXcd(UINT,UINT,UINT)>,
        std::string> testset;
    std::vector<testset> test_list;
    test_list.push_back(std::make_tuple(CNOT_gate, get_eigen_matrix_full_qubit_CNOT, "CNOT"));
    test_list.push_back(std::make_tuple(CZ_gate, get_eigen_matrix_full_qubit_CZ, "CZ"));
    test_list.push_back(std::make_tuple(SWAP_gate, get_eigen_matrix_full_qubit_SWAP, "SWAP"));

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        for (auto tup : test_list) {
            std::random_shuffle(index_list.begin(), index_list.end());
            target = index_list[0];
            control = index_list[1];
            ASSERT_NE(target, control);
            auto func = std::get<0>(tup);
            auto mat_func = std::get<1>(tup);
            auto name = std::get<2>(tup);

            func(control, target, state, dim);
            test_state = mat_func(control,target,n) * test_state;
            state_equal(state, test_state, dim, name);
        }
    }
    release_quantum_state(state);
}

TEST(UpdateTest, SingleQubitPauliTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    UINT target, pauli;
    double angle;

    Eigen::MatrixXcd Identity(2, 2);
    Identity << 1, 0, 0, 1;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        /* single qubit Pauli gate */
        target = rand_int(n);
        pauli = rand_int(4);
        single_qubit_Pauli_gate(target, pauli, state, dim);
        test_state = get_expanded_eigen_matrix_with_identity(target, get_eigen_matrix_single_Pauli(pauli), n) * test_state;
        state_equal(state, test_state, dim, "single Pauli gate");

        target = rand_int(n);
        pauli = rand_int(4);
        angle = rand_real();
        single_qubit_Pauli_rotation_gate(target, pauli, angle, state, dim);
        test_state = get_expanded_eigen_matrix_with_identity(target, cos(angle/2)*Identity + 1.i * sin(angle/2) * get_eigen_matrix_single_Pauli(pauli), n) * test_state;
        state_equal(state, test_state, dim, "single rotation Pauli gate");
    }
    release_quantum_state(state);
}

TEST(UpdateTest, MultiQubitPauliTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    UINT pauli;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        // multi pauli whole
        std::vector<UINT> pauli_whole, pauli_partial, pauli_partial_index;
        std::vector<std::pair<UINT, UINT>> pauli_partial_pair;

        pauli_whole.resize(n);
        for (UINT i = 0; i < n; ++i) {
            pauli_whole[i] = rand_int(4);
        }
        multi_qubit_Pauli_gate_whole_list(pauli_whole.data(), n, state, dim);
        test_state = get_eigen_matrix_full_qubit_pauli(pauli_whole) * test_state;
        state_equal(state, test_state, dim, "multi Pauli whole gate");

        // multi pauli partial
        pauli_partial.clear();
        pauli_partial_index.clear();
        pauli_partial_pair.clear();
        for (UINT i = 0; i < n; ++i) {
            pauli = rand_int(4);
            pauli_whole[i] = pauli;
            if (pauli != 0) {
                pauli_partial_pair.push_back(std::make_pair(i, pauli));
            }
        }
        std::random_shuffle(pauli_partial_pair.begin(), pauli_partial_pair.end());
        for (auto val : pauli_partial_pair) {
            pauli_partial_index.push_back(val.first);
            pauli_partial.push_back(val.second);
        }
        multi_qubit_Pauli_gate_partial_list(pauli_partial_index.data(), pauli_partial.data(), (UINT)pauli_partial.size(), state, dim);
        test_state = get_eigen_matrix_full_qubit_pauli(pauli_whole) * test_state;
        state_equal(state, test_state, dim, "multi Pauli partial gate");
    }
    release_quantum_state(state);
}

TEST(UpdateTest, MultiQubitPauliRotationTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    UINT pauli;
    double angle;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {

        std::vector<UINT> pauli_whole, pauli_partial, pauli_partial_index;
        std::vector<std::pair<UINT, UINT>> pauli_partial_pair;

        // multi pauli rotation whole
        pauli_whole.resize(n);
        for (UINT i = 0; i < n; ++i) {
            pauli_whole[i] = rand_int(4);
        }
        angle = rand_real();
        multi_qubit_Pauli_rotation_gate_whole_list(pauli_whole.data(), n, angle, state, dim);
        test_state = (cos(angle/2)*whole_I + 1.i * sin(angle/2)* get_eigen_matrix_full_qubit_pauli(pauli_whole)) * test_state;
        state_equal(state, test_state, dim, "multi Pauli rotation whole gate");

        // multi pauli rotation partial
        pauli_partial.clear();
        pauli_partial_index.clear();
        pauli_partial_pair.clear();
        for (UINT i = 0; i < n; ++i) {
            pauli = rand_int(4);
            pauli_whole[i] = pauli;
            if (pauli != 0) {
                pauli_partial_pair.push_back(std::make_pair(i, pauli));
            }
        }
        std::random_shuffle(pauli_partial_pair.begin(), pauli_partial_pair.end());
        for (auto val : pauli_partial_pair) {
            pauli_partial_index.push_back(val.first);
            pauli_partial.push_back(val.second);
        }
        angle = rand_real();
        multi_qubit_Pauli_rotation_gate_partial_list(pauli_partial_index.data(), pauli_partial.data(), (UINT)pauli_partial.size(), angle, state, dim);
        test_state = (cos(angle/2)*whole_I + 1.i * sin(angle/2)* get_eigen_matrix_full_qubit_pauli(pauli_whole)) * test_state;
        state_equal(state, test_state, dim, "multi Pauli rotation partial gate");
    }
    release_quantum_state(state);
}

TEST(UpdateTest, SingleDenseMatrixTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U;

    UINT target;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        // single qubit dense matrix gate
        // NOTE: Eigen uses column major by default. To use raw-data of eigen matrix, we need to specify RowMajor.
        target = rand_int(n);
        U = get_eigen_matrix_random_single_qubit_unitary();
        single_qubit_dense_matrix_gate(target, (CTYPE*)U.data(), state, dim);
        test_state = get_expanded_eigen_matrix_with_identity(target, U, n) * test_state;
        state_equal(state, test_state, dim, "single dense gate");
    }
    release_quantum_state(state);
}

TEST(UpdateTest, SingleDiagonalMatrixTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    Eigen::MatrixXcd Identity(2, 2),Z(2, 2);
    Identity << 1, 0, 0, 1;
    Z << 1, 0, 0, -1;

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U;

    UINT target;
    double icoef, zcoef, norm;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        // single qubit diagonal matrix gate
        target = rand_int(n);
        icoef = rand_real(); zcoef = rand_real();
        norm = sqrt(icoef * icoef + zcoef * zcoef);
        icoef /= norm; zcoef /= norm;
        U = icoef * Identity + 1.i*zcoef * Z;
        Eigen::VectorXcd diag = U.diagonal();
        single_qubit_diagonal_matrix_gate(target, (CTYPE*)diag.data(), state, dim);
        test_state = get_expanded_eigen_matrix_with_identity(target, U, n) * test_state;
        state_equal(state, test_state, dim, "single diagonal gate");
    }
    release_quantum_state(state);
}

TEST(UpdateTest, SinglePhaseMatrixTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U;

    UINT target;
    double angle;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        // single qubit phase matrix gate
        target = rand_int(n);
        angle = rand_real();
        U << 1, 0, 0, cos(angle) + 1.i*sin(angle);
        single_qubit_phase_gate(target, cos(angle) + 1.i*sin(angle), state, dim);
        test_state = get_expanded_eigen_matrix_with_identity(target, U, n) * test_state;
        state_equal(state, test_state, dim, "single phase gate");
    }
    release_quantum_state(state);
}

TEST(UpdateTest, SingleQubitControlSingleQubitDenseMatrixTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    Eigen::MatrixXcd P0(2, 2), P1(2, 2);
    P0 << 1, 0, 0, 0;
    P1 << 0, 0, 0, 1;

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U;

    UINT target,control;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        // single qubit control-1 single qubit gate
        target = rand_int(n);
        control = rand_int(n - 1);
        if (control >= target) control++;
        U = get_eigen_matrix_random_single_qubit_unitary();
        single_qubit_control_single_qubit_dense_matrix_gate(control, 1, target, (CTYPE*)U.data(), state, dim);
        test_state = (get_expanded_eigen_matrix_with_identity(control, P0, n) + get_expanded_eigen_matrix_with_identity(control, P1, n)*get_expanded_eigen_matrix_with_identity(target, U, n)) * test_state;
        state_equal(state, test_state, dim, "single qubit control sinlge qubit dense gate");

        // single qubit control-0 single qubit gate
        target = rand_int(n);
        control = rand_int(n - 1);
        if (control >= target) control++;
        U = get_eigen_matrix_random_single_qubit_unitary();
        single_qubit_control_single_qubit_dense_matrix_gate(control, 0, target, (CTYPE*)U.data(), state, dim);
        test_state = (get_expanded_eigen_matrix_with_identity(control, P1, n) + get_expanded_eigen_matrix_with_identity(control, P0, n)*get_expanded_eigen_matrix_with_identity(target, U, n)) * test_state;
        state_equal(state, test_state, dim, "single qubit control sinlge qubit dense gate");
    }
    release_quantum_state(state);
}

TEST(UpdateTest, TwoQubitControlSingleQubitDenseMatrixTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    std::vector<UINT> index_list;
    for (UINT i = 0; i < n; ++i) index_list.push_back(i);

    Eigen::MatrixXcd P0(2, 2), P1(2, 2);
    P0 << 1, 0, 0, 0;
    P1 << 0, 0, 0, 1;

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U;

    UINT target;
    UINT controls[2];

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        // two qubit control-10 single qubit gate
        std::random_shuffle(index_list.begin(), index_list.end());
        target = index_list[0];
        controls[0] = index_list[1];
        controls[1] = index_list[2];

        U = get_eigen_matrix_random_single_qubit_unitary();
        UINT mvalues[2] = { 1,0 };
        multi_qubit_control_single_qubit_dense_matrix_gate(controls, mvalues, 2, target, (CTYPE*)U.data(), state, dim);
        test_state = (
            get_expanded_eigen_matrix_with_identity(controls[0], P0, n)*get_expanded_eigen_matrix_with_identity(controls[1], P0, n) +
            get_expanded_eigen_matrix_with_identity(controls[0], P0, n)*get_expanded_eigen_matrix_with_identity(controls[1], P1, n) +
            get_expanded_eigen_matrix_with_identity(controls[0], P1, n)*get_expanded_eigen_matrix_with_identity(controls[1], P0, n)*get_expanded_eigen_matrix_with_identity(target, U, n) +
            get_expanded_eigen_matrix_with_identity(controls[0], P1, n)*get_expanded_eigen_matrix_with_identity(controls[1], P1, n)
            ) * test_state;
        state_equal(state, test_state, dim, "two qubit control sinlge qubit dense gate");

    }
    release_quantum_state(state);
}

TEST(UpdateTest, TwoQubitDenseMatrixTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    std::vector<UINT> index_list;
    for (UINT i = 0; i < n; ++i) index_list.push_back(i);

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U,U2;
    Eigen::Matrix<std::complex<double>, 4,4, Eigen::RowMajor> Umerge;

    UINT targets[2];

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {

        // two qubit dense matrix gate
        U = get_eigen_matrix_random_single_qubit_unitary();
        U2 = get_eigen_matrix_random_single_qubit_unitary();

        std::random_shuffle(index_list.begin(), index_list.end());

        targets[0] = index_list[0];
        targets[1] = index_list[1];
        Umerge = kronecker_product(U2, U);
        // the below two lines are equivalent to the above two line
        //UINT targets_rev[2] = { targets[1], targets[0] };
        //Umerge = kronecker_product(U, U2);
        test_state = get_expanded_eigen_matrix_with_identity(targets[1], U2, n) * get_expanded_eigen_matrix_with_identity(targets[0], U, n) * test_state;
        multi_qubit_dense_matrix_gate(targets, 2, (CTYPE*)Umerge.data(), state, dim);
        state_equal(state, test_state, dim, "two-qubit separable dense gate");
    }
    release_quantum_state(state);
}

TEST(UpdateTest, TwoQubitDenseMatrixTest2) {
	const UINT n = 6;
	const ITYPE dim = 1ULL << n;
	const UINT max_repeat = 10;

	std::vector<UINT> index_list;
	for (UINT i = 0; i < n; ++i) index_list.push_back(i);

	Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U, U2;
	Eigen::Matrix<std::complex<double>, 4, 4, Eigen::RowMajor> Umerge;

	UINT targets[2];

	auto state = allocate_quantum_state(dim);
	initialize_Haar_random_state(state, dim);
	Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
	for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

	Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

	for (UINT rep = 0; rep < max_repeat; ++rep) {

		// two qubit dense matrix gate
		U = get_eigen_matrix_random_single_qubit_unitary();
		U2 = get_eigen_matrix_random_single_qubit_unitary();

		std::random_shuffle(index_list.begin(), index_list.end());

		targets[0] = index_list[0];
		targets[1] = index_list[1];
		Umerge = kronecker_product(U2, U);
		// the below two lines are equivalent to the above two line
		//UINT targets_rev[2] = { targets[1], targets[0] };
		//Umerge = kronecker_product(U, U2);
		test_state = get_expanded_eigen_matrix_with_identity(targets[1], U2, n) * get_expanded_eigen_matrix_with_identity(targets[0], U, n) * test_state;
		two_qubit_dense_matrix_gate(targets[0], targets[1], (CTYPE*)Umerge.data(), state, dim);
		state_equal(state, test_state, dim, "two-qubit separable dense gate");
	}
	release_quantum_state(state);
}

TEST(UpdateTest, SingleQubitControlTwoQubitDenseMatrixTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    std::vector<UINT> index_list;
    for (UINT i = 0; i < n; ++i) index_list.push_back(i);

    Eigen::MatrixXcd P0(2, 2), P1(2, 2);
    P0 << 1, 0, 0, 0;
    P1 << 0, 0, 0, 1;

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U, U2;
    Eigen::Matrix<std::complex<double>, 4, 4, Eigen::RowMajor> Umerge;

    UINT targets[2], control;

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {
        // single qubit 1-controlled qubit dense matrix gate
        U = get_eigen_matrix_random_single_qubit_unitary();
        U2 = get_eigen_matrix_random_single_qubit_unitary();
        std::random_shuffle(index_list.begin(), index_list.end());
        targets[0] = index_list[0];
        targets[1] = index_list[1];
        control = index_list[2];

        Umerge = kronecker_product(U2, U);
        test_state = (get_expanded_eigen_matrix_with_identity(control, P0, n) + get_expanded_eigen_matrix_with_identity(control, P1, n)*get_expanded_eigen_matrix_with_identity(targets[1], U2, n) * get_expanded_eigen_matrix_with_identity(targets[0], U, n)) * test_state;
        single_qubit_control_multi_qubit_dense_matrix_gate(control, 1, targets, 2, (CTYPE*)Umerge.data(), state, dim);
        state_equal(state, test_state, dim, "single qubit control two-qubit separable dense gate");
    }
    release_quantum_state(state);
}

TEST(UpdateTest, TwoQubitControlTwoQubitDenseMatrixTest) {
    const UINT n = 6;
    const ITYPE dim = 1ULL << n;
    const UINT max_repeat = 10;

    std::vector<UINT> index_list;
    for (UINT i = 0; i < n; ++i) index_list.push_back(i);

    Eigen::MatrixXcd P0(2, 2), P1(2, 2);
    P0 << 1, 0, 0, 0;
    P1 << 0, 0, 0, 1;

    Eigen::Matrix<std::complex<double>, 2, 2, Eigen::RowMajor> U, U2;
    Eigen::Matrix<std::complex<double>, 4, 4, Eigen::RowMajor> Umerge;

    UINT targets[2], controls[2],mvalues[2];

    auto state = allocate_quantum_state(dim);
    initialize_Haar_random_state(state, dim);
    Eigen::VectorXcd test_state = Eigen::VectorXcd::Zero(dim);
    for (ITYPE i = 0; i < dim; ++i) test_state[i] = state[i];

    Eigen::MatrixXcd whole_I = Eigen::MatrixXcd::Identity(dim, dim);

    for (UINT rep = 0; rep < max_repeat; ++rep) {

        // two qubit control-11 two qubit gate
        U = get_eigen_matrix_random_single_qubit_unitary();
        U2 = get_eigen_matrix_random_single_qubit_unitary();
        std::random_shuffle(index_list.begin(), index_list.end());
        targets[0] = index_list[0];
        targets[1] = index_list[1];
        controls[0] = index_list[2];
        controls[1] = index_list[3];

        mvalues[0] = 1; mvalues[1] = 1;
        Umerge = kronecker_product(U2, U);
        multi_qubit_control_multi_qubit_dense_matrix_gate(controls, mvalues, 2, targets, 2, (CTYPE*)Umerge.data(), state, dim);
        test_state = (
            get_expanded_eigen_matrix_with_identity(controls[0], P0, n)*get_expanded_eigen_matrix_with_identity(controls[1], P0, n) +
            get_expanded_eigen_matrix_with_identity(controls[0], P0, n)*get_expanded_eigen_matrix_with_identity(controls[1], P1, n) +
            get_expanded_eigen_matrix_with_identity(controls[0], P1, n)*get_expanded_eigen_matrix_with_identity(controls[1], P0, n) +
            get_expanded_eigen_matrix_with_identity(controls[0], P1, n)*get_expanded_eigen_matrix_with_identity(controls[1], P1, n)*get_expanded_eigen_matrix_with_identity(targets[0], U, n)*get_expanded_eigen_matrix_with_identity(targets[1], U2, n)
            ) * test_state;
        state_equal(state, test_state, dim, "two qubit control two qubit dense gate");
    }
    release_quantum_state(state);
}


#include <boost/math/constants/constants.hpp>

#include <TMath.h>

#include "IbdInteraction.hh"
#include "IbdZeroOrder.hh"
#include "PDGVariables.hh"

#include <iostream>

constexpr double pi = boost::math::constants::pi<double>();

IbdZeroOrder::IbdZeroOrder()
{
  transformation_("Enu")
    .input("Ee")
    .output("Enu")
    .func(&IbdZeroOrder::calcEnu);
  transformation_("xsec")
    .input("Ee")
    .output("xsec")
    .func(&IbdZeroOrder::calcXsec);
}

IbdZeroOrder::IbdZeroOrder(bool useEnu): IbdZeroOrder() {
    this->useEnu = useEnu;
}

void IbdZeroOrder::calcEnu(FunctionArgs fargs) {
  fargs.rets[0].x = fargs.args[0].x + m_DeltaNP;
}

void IbdZeroOrder::calcXsec(FunctionArgs fargs) {
  Eigen::ArrayXd Ee;
  if (this->useEnu) {
      Ee = fargs.args[0].x - m_DeltaNP;
  } else {
      Ee = fargs.args[0].x;
  }

  const double MeV2J = 1.E6 * TMath::Qe();
  const double J2MeV = 1./MeV2J;

  const double MeV2cm = pow(TMath::Hbar()*TMath::C()*J2MeV, 2) * 1.E4;

  Eigen::ArrayXd pe = (Ee.square() - m_pdg->ElectronMass*m_pdg->ElectronMass).sqrt();
/* Sanity check to null all possible nans */
  std::transform(pe.data(), pe.data() + pe.size(), pe.data(),
                [](double x){return (!std::isnan(x) ? x : 0.);});
  auto coeff = 2.*pi*pi /
    (std::pow(m_pdg->ElectronMass, 5) * PhaseFactor * m_pdg->NeutronLifeTime/(1.E-6*TMath::Hbar()/TMath::Qe()));
  fargs.rets[0].x = MeV2cm * coeff*Ee*pe;
}


/*
** EPITECH PROJECT, 2017
** ECS Engine
** File description:
** Thomas Arbona
*/
#pragma once

#include <vector>
#include <irrlicht/irrlicht.h>
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include "../core/Scene.hpp"

namespace engine {

    struct TransformComponent;
    struct HitboxComponent;
    struct PhysicsComponent;
    class Entity;

    using Point = boost::geometry::model::d2::point_xy<float>;
    using Polygon = boost::geometry::model::polygon<Point>;
    using Segment = boost::geometry::model::referring_segment<const Point>;

    using Vec2D = irr::core::vector2d<float>;

    struct Segment3D {
        irr::core::vector3df p1;
        irr::core::vector3df p2;
    };

    struct Manifold {
        bool isCollide = false;
        bool hasError = false;
        Vec2D normal{0.f, 0.f};
    };

	class GeometryHelper {
	public:
        static std::vector<Segment> getCombinedSegments(Polygon const& p1, Polygon const& p2);
        static bool segmentsAreCollinear(Segment const& s1, Segment const& s2);
        static void transformHitbox(HitboxComponent& hitbox, TransformComponent const& transform);
        static Manifold polygonCollide(Entity const& entity, Entity const& entity2, float rebound, int call = 0);
        static Manifold polygonCollideChilds(Entity const& entity, Entity const& entity2, float rebound);
        static bool simplePolygonCollide(Entity const& entity, Entity const& entity2);
        static Point mergeSegmentsIntoVector(std::vector<Segment> const& segments);
        static bool AABBCollide(HitboxComponent const& h1, HitboxComponent const& h2);
		static Entity createBlastPolygon(Scene& scene, float x, float y, float radius);

        static const float epsilon;

	private:
        GeometryHelper() = default;
        ~GeometryHelper() = default;
	};
}


//==================================================================================================
/*
  Copyright 2017 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
//! [remainders]
#include <boost/simd/arithmetic.hpp>
#include <boost/simd/pack.hpp>
#include <iostream>

namespace bs =  boost::simd;
using pack_ft =  bs::pack <float, 8>;

int main()
{
  pack_ft p = {-4, -3, -2, -1, 1, 2, 3, 4};
  pack_ft q = { 5,  2,  3,  2, 2, 3, 2, 5};
  std::cout << " p = " << p << std::endl
            << " q = " << q << std::endl
            <<  " -> bs::rem(bs::ceil,      p, q) = " << bs::rem(bs::ceil,      p, q) << std::endl
            <<  " -> bs::rem(bs::floor,     p, q) = " << bs::rem(bs::floor,     p, q) << std::endl
            <<  " -> bs::rem(bs::fix,       p, q) = " << bs::rem(bs::fix,       p, q) << std::endl
            <<  " -> bs::rem(bs::round,     p, q) = " << bs::rem(bs::round,     p, q) << std::endl
            <<  " -> bs::rem(bs::nearbyint, p, q) = " << bs::rem(bs::nearbyint, p, q) << std::endl;
  return 0;
}
//! [remainders]


#include <stan/math/fwd/scal.hpp>
#include <gtest/gtest.h>
#include <boost/math/special_functions/fpclassify.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <test/unit/math/fwd/scal/fun/nan_util.hpp>

TEST(AgradFwdGammaP, gamma_p){
  using stan::math::fvar;
  using stan::math::gamma_p;
  using boost::math::gamma_p;

  fvar<double> x(0.5);
  x.d_ = 1.0;
  fvar<double> y (1.0);
  y.d_ = 1.0;

  fvar<double> a = gamma_p(x,y);
  EXPECT_FLOAT_EQ(gamma_p(0.5,1.0), a.val_);
  EXPECT_FLOAT_EQ(-0.18228334, a.d_);

  double z = 1.0;
  double w = 0.5;

  a = gamma_p(x,z);
  EXPECT_FLOAT_EQ(gamma_p(0.5,1.0), a.val_);
  EXPECT_FLOAT_EQ(-0.389837, a.d_);

  a = gamma_p(w,y);
  EXPECT_FLOAT_EQ(gamma_p(0.5,1.0), a.val_);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), a.d_);

  EXPECT_THROW(gamma_p(-x,y), std::domain_error);
  EXPECT_THROW(gamma_p(x,-y), std::domain_error);
}

TEST(AgradFwdGammaP, FvarFvarDouble) {
  using stan::math::fvar;
  using boost::math::gamma_p;

  fvar<fvar<double> > x;
  x.val_.val_ = 0.5;
  x.val_.d_ = 1.0;

  fvar<fvar<double> > y;
  y.val_.val_ = 1.0;
  y.d_.val_ = 1.0;

  fvar<fvar<double> > a = gamma_p(x,y);

  EXPECT_FLOAT_EQ(gamma_p(0.5,1.0), a.val_.val_);
  EXPECT_FLOAT_EQ(-0.38983709, a.val_.d_);
  EXPECT_FLOAT_EQ(boost::math::gamma_p_derivative(0.5,1.0), a.d_.val_);
  EXPECT_FLOAT_EQ(0.40753385, a.d_.d_);
}

struct gamma_p_fun {
  template <typename T0, typename T1>
  inline
  typename boost::math::tools::promote_args<T0,T1>::type
  operator()(const T0 arg1,
             const T1 arg2) const {
    return gamma_p(arg1,arg2);
  }
};

TEST(AgradFwdGammaP, nan) {
  gamma_p_fun gamma_p_;
  test_nan_fwd(gamma_p_,3.0,5.0,false);
}


//    boost asinh.hpp header file

//  (C) Copyright Eric Ford & Hubert Holin 2001.
//  (C) Copyright John Maddock 2008.
//  Distributed under the Boost Software License, Version 1.0. (See
//  accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

// See http://www.boost.org for updates, documentation, and revision history.

#ifndef BOOST_ASINH_HPP
#define BOOST_ASINH_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <boost/config/no_tr1/cmath.hpp>
#include <boost/config.hpp>
#include <boost/math/tools/precision.hpp>
#include <boost/math/special_functions/math_fwd.hpp>
#include <boost/math/special_functions/sqrt1pm1.hpp>
#include <boost/math/special_functions/log1p.hpp>

// This is the inverse of the hyperbolic sine function.

namespace boost
{
    namespace math
    {
       namespace detail{
#if defined(__GNUC__) && (__GNUC__ < 3)
        // gcc 2.x ignores function scope using declarations,
        // put them in the scope of the enclosing namespace instead:

        using    ::std::abs;
        using    ::std::sqrt;
        using    ::std::log;

        using    ::std::numeric_limits;
#endif

        template<typename T, class Policy>
        inline T    asinh_imp(const T x, const Policy& pol)
        {
            BOOST_MATH_STD_USING

            if        (x >= tools::forth_root_epsilon<T>())
            {
               if        (x > 1 / tools::root_epsilon<T>())
                {
                    // http://functions.wolfram.com/ElementaryFunctions/ArcSinh/06/01/06/01/0001/
                    // approximation by laurent series in 1/x at 0+ order from -1 to 1
                    return log(x * 2) + 1/ (4 * x * x);
                }
                else if(x < 0.5f)
                {
                   // As below, but rearranged to preserve digits:
                   return boost::math::log1p(x + boost::math::sqrt1pm1(x * x, pol), pol);
                }
                else
                {
                    // http://functions.wolfram.com/ElementaryFunctions/ArcSinh/02/
                    return( log( x + sqrt(x*x+1) ) );
                }
            }
            else if    (x <= -tools::forth_root_epsilon<T>())
            {
                return(-asinh(-x));
            }
            else
            {
                // http://functions.wolfram.com/ElementaryFunctions/ArcSinh/06/01/03/01/0001/
                // approximation by taylor series in x at 0 up to order 2
                T    result = x;

                if    (abs(x) >= tools::root_epsilon<T>())
                {
                    T    x3 = x*x*x;

                    // approximation by taylor series in x at 0 up to order 4
                    result -= x3/static_cast<T>(6);
                }

                return(result);
            }
        }
       }

        template<typename T>
        inline typename tools::promote_args<T>::type asinh(T x)
        {
           return boost::math::asinh(x, policies::policy<>());
        }
        template<typename T, typename Policy>
        inline typename tools::promote_args<T>::type asinh(T x, const Policy&)
        {
            typedef typename tools::promote_args<T>::type result_type;
            typedef typename policies::evaluation<result_type, Policy>::type value_type;
            typedef typename policies::normalise<
               Policy,
               policies::promote_float<false>,
               policies::promote_double<false>,
               policies::discrete_quantile<>,
               policies::assert_undefined<> >::type forwarding_policy;
           return policies::checked_narrowing_cast<result_type, forwarding_policy>(
              detail::asinh_imp(static_cast<value_type>(x), forwarding_policy()),
              "boost::math::asinh<%1%>(%1%)");
        }

    }
}

#endif /* BOOST_ASINH_HPP */


#include <array>
#include <vector>
#include <string>
#include <iostream>
#include <fstream>
#include <sstream>
#include <cstdlib>
#include <math.h>
#include <Eigen/Dense>

int print_vector_int(std::vector<int> A) {
   int i;
   for (i=0; i<A.size(); i=i+1) {
      std::cout << A[i] <<  " " ;
   }
   std::cout << " " << std::endl;
   std::cout << " " << std::endl;
   return 0;
}

int print_vector_double(std::vector<double> A) {
   int i;
   for (i=0; i<A.size(); i=i+1) {
      std::cout << A[i] <<  " " ;
   }
   std::cout << " " << std::endl;
   std::cout << " " << std::endl;
   return 0;
}

int print_vector_string(std::vector<std::string> A) {
   int i;
   for (i=0; i<A.size(); i=i+1) {
      std::cout << A[i] << " ";
   }
   std::cout << " " << std::endl;
   std::cout << " " << std::endl;
   return 0;
}

int print_matrix_int(std::vector<std::vector<int> > A) {
   int i, j;
   for (i=0; i<A.size(); i=i+1) {
      for (j=0; j<A[i].size(); j=j+1) {
         std::cout << A[i][j] << " ";
      }
      std::cout << " " << std::endl;
   }
   std::cout << " " << std::endl;
   return 0;
}

bool fexists(const char *filename) {
   std::ifstream ifile(filename);
   return ifile.good();
}

int if_file_exist_delete (std::string filename) {
   if (fexists(filename.c_str())) {
      if (std::remove(filename.c_str()) != 0) {
          std::cout << "failed to remove " << filename << std::endl;
          exit(1);
       }
       else {
          std::cout << filename << " found and deleted " << std::endl;
       }
   }
   return 0;
}

int x_rotation_matrix (double cosx, double sinx, Eigen::Matrix3d& Rx) {

   Rx(0,0)=1;
   Rx(0,1)=0;
   Rx(0,2)=0;
   Rx(1,0)=0;
   Rx(1,1)=cosx;
   Rx(1,2)=-sinx;
   Rx(2,0)=0;
   Rx(2,1)=sinx;
   Rx(2,2)=cosx;

   std::cout << "Rx " << Rx << std::endl;

   return 0;
}

int y_rotation_matrix (double cosy, double siny, Eigen::Matrix3d& Ry) {

   Ry(0,0)=cosy;
   Ry(0,1)=0;
   Ry(0,2)=-siny;
   Ry(1,0)=0;
   Ry(1,1)=1;
   Ry(1,2)=0;
   Ry(2,0)=siny;
   Ry(2,1)=0;
   Ry(2,2)=cosy;

   std::cout << "Ry " << Ry << std::endl;

   return 0;
}

int align (Eigen::Vector3d vec_in, Eigen::MatrixXd& coords) {

   double cosx, sinx, cosy, siny;
   Eigen::Matrix3d Rx, Ry;
   Eigen::Vector3d vecz;
   Eigen::MatrixXd coords_t(coords.cols(),coords.rows());
   coords_t = coords.transpose();

   if (vec_in(0,0) == 0) {
      if (vec_in(1,0) != 0) { // if vec_in = [0,y,z]

         // find matrix for rotation about the x-axis:
         cosx = vec_in(2,0)/sqrt( pow(vec_in(1,0),2) + pow(vec_in(2,0),2) );
         sinx = vec_in(1,0)/sqrt( pow(vec_in(1,0),2) + pow(vec_in(2,0),2) );
         x_rotation_matrix(cosx, sinx, Rx);

         // rotate space about the x - axis to bring vec_in
         // in the xz-plane:
         coords_t = Rx*coords_t;
         vecz = Rx*vec_in;
      }
      else if (vec_in(1,0) == 0) {// [0,0,z]
         // all is good
         vecz = vec_in;
      }
   }
   if (vec_in(1,0) == 0) {
      if (vec_in(0,0) != 0) { // [x, 0, z]

         // find matrix for rotation about the y-axis:
         cosy = vec_in(2,0)/sqrt( pow(vec_in(0,0),2) + pow(vec_in(2,0),2) );
         siny = vec_in(0,0)/sqrt( pow(vec_in(0,0),2) + pow(vec_in(2,0),2) );
         y_rotation_matrix (cosy, siny, Ry);

         // rotate space around the y - axis, so that the
         // rotation axis lies along the positive z - axis:
         coords_t = Ry*coords_t;
         vecz = Ry*vec_in; // should have the form [0,0,z]
      }
   }
   if (vec_in(0,0) != 0 ) {
      if (vec_in(1,0) != 0) { // [x, y, z]

         Eigen::Vector3d vecxz;

         // find matrix for rotation about the x-axis:
         cosx = vec_in(2,0)/sqrt( pow(vec_in(1,0),2) + pow(vec_in(2,0),2) );
         sinx = vec_in(1,0)/sqrt( pow(vec_in(1,0),2) + pow(vec_in(2,0),2) );
         x_rotation_matrix (cosx, sinx, Rx);

         // rotate space about the x - axis to bring vec_in
         // in the xz-plane:
         std::cout << Ry.rows() << Ry.cols() << coords.rows() << coords.cols()    ;
         coords_t = Rx*coords_t;
         vecxz = Rx*vec_in;

         //find matrix for rotation about the y-axis
         cosy = vecxz(2,0)/sqrt( pow(vecxz(0,0),2) + pow(vecxz(2,0),2) );
         siny = vecxz(0,0)/sqrt( pow(vecxz(0,0),2) + pow(vecxz(2,0),2) );
         y_rotation_matrix (cosy, siny, Ry);

         // rotate space around the y - axis, so that the rotation axis lies
         // along the positive z - axis:
         std::cout << Ry.rows() << Ry.cols() << coords.rows() << coords.cols();
         coords_t = Ry*coords_t;
         vecz = Ry*vecxz; //should have the form [0,0,z]
      }
   }
   coords = coords_t.transpose();
   std::cout << "vecz: " << vecz << std::endl;
   return 0;
}

int periodic_table(std::string& lable, double& mass) {

   int k;
	std::vector<std::string> lables{"C", "S"};
	std::vector<double> masses{12.0107, 32.065};

   std::cout << "lable: " << lable << "end" << std::endl;

	for (k=0; k<lables.size(); k=k+1) {
      if (lable == lables[k]) {
         mass = masses[k];
      }
   }

   return(0);
}

int read_input_file(int& nmols, std::vector<std::string>& filenames, \
      std::vector<int>& nrings_per_molecule, \
      std::vector<std::vector<int>>& rings_atoms) {

   std::string line, temp;
   char test_char;
   int nrings_total, nfile, counts, finds_nl, rr, temp_int;

   // check input file:
   // for line n
   // if old = m new != f wrong
   // if old = f new != r wrong
   // if old = r new != r or m wrong
   // if wrong cout "there is a mistake at line n

   //----------------------------------
   // count number of molecules,
   // and the total number of rings:
   //----------------------------------

   std::ifstream inputfile;
   inputfile.open("input");
   nmols = 0;
   finds_nl = 0; // finds new line
   nrings_total = 0;
   while (!inputfile.eof()) {
      inputfile.get(test_char);
      if (finds_nl == 0) { // if this is the first character in the line
         if (test_char == 'm') { // and the first character is 'm'
            nmols = nmols + 1;
         }
         if (test_char == 'r') {
            nrings_total = nrings_total + 1;
         }
      }
      finds_nl = finds_nl + 1; // because it's probably not the
                                       // end of the line
      if (test_char == '\n') { // though, if it is the end of the line:
         finds_nl = 0;
      }
   }
   std::cout << "nrings total: " << nrings_total << std::endl;

   //---------------------------------------------
   // get the filenames of the molecules,
   // count the total numbers of rings, and the
   // number of rings per molecule:
   //---------------------------------------------

   filenames.resize(nmols);
   nrings_per_molecule.resize(nmols);
   rings_atoms.resize(nrings_total);
   nfile = -1; // because c++ array numbering starts from 0
   rr = 0; // the current ring being read
   finds_nl = 0; // finds new line
   inputfile.clear(); // To clear the EOF from previously
   inputfile.seekg(0, std::ios::beg); // set to beggining of file
   while (!inputfile.eof()) {
      inputfile.get(test_char);
      if (finds_nl == 0) { // if this is the first character in the line
         if (test_char == 'm') { // we have a new molecule
            nfile = nfile + 1;
            nrings_per_molecule[nfile] = 0; // initialize to 0 rings
         }
         if (test_char == 'f') { // the filename of the new molecule is
                                 // written here
            getline(inputfile,line); // get this line
            std::stringstream ssin(line); // break up line in string stream
            counts = 1;
            while (ssin.good()){
               ssin >> temp;
               if (counts = 2) {
                  filenames[nfile] = temp;
               }
               counts = counts + 1;
            }
            // go back by 1 character so that get reads '\n' from
            // this line which was read by getline:
            inputfile.seekg(-1, std::ios::cur);
         }
         if (test_char == 'r') {
            // there is another ring for this molecule:
            nrings_per_molecule[nfile] = nrings_per_molecule[nfile] + 1;
            // get the atoms that make up this ring
            getline(inputfile,line); // get this line
            std::stringstream ssin(line); // break up line in string stream
            counts = 1;
            while (ssin.good()){
               ssin >> temp;
               std::cout << temp << " ";
               if (counts > 1) {
                  temp_int = std::atoi(temp.c_str());
                  rings_atoms[rr].push_back(temp_int);
               }
               counts = counts + 1;
            }
            std::cout << " " << std::endl;
            // move on to next ring
            rr = rr + 1;
            // go back by 1 character so that get reads '\n' from
            // this line which was read by getline:
            inputfile.seekg(-1, std::ios::cur);
         }
      }
      finds_nl = finds_nl + 1; // because it's probably not the
                                       // end of the line
      if (test_char == '\n') { // though, if it is the end of the line:
         finds_nl = 0;
      }
   }

   inputfile.close();
   return 0;
}


/**
 *  .file test/oglplus/angle.cpp
 *  .brief Test case for Angle class and related functionality.
 *
 *  .author Matus Chochlik
 *
 *  Copyright 2011-2019 Matus Chochlik. Distributed under the Boost
 *  Software License, Version 1.0. (See accompanying file
 *  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
 */
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE OGLPLUS_Angle
#include <boost/test/unit_test.hpp>

#include <oglplus/gl.hpp>
#include <oglplus/math/angle.hpp>

BOOST_AUTO_TEST_SUITE(Angle)

BOOST_AUTO_TEST_CASE(Angle_default_construction) {
    oglplus::Angle<float> af;
    oglplus::Angle<double> ad;
}

BOOST_AUTO_TEST_CASE(Angle_construction) {
    using Anglef = oglplus::Angle<float>;
    Anglef a1 = Anglef::Degrees(90);
    Anglef a2 = Anglef::Radians(oglplus::math::HalfPi());
    Anglef a3 = oglplus::Degrees(90);
    Anglef a4 = oglplus::Radians(oglplus::math::HalfPi());
    Anglef a5 = oglplus::RightAngles(1.0f);
    Anglef a6 = oglplus::FullCircles(1.0f);
    Anglef a7 = a6;
}

BOOST_AUTO_TEST_CASE(Angle_value) {
    using Anglef = oglplus::Angle<float>;

    Anglef a1 = Anglef::Radians(oglplus::math::HalfPi());
    Anglef a2 = Anglef::Radians(oglplus::math::Pi());
    Anglef a3 = Anglef::Radians(oglplus::math::TwoPi());

    BOOST_CHECK_EQUAL(a1.ValueInDegrees(), 90);
    BOOST_CHECK_EQUAL(a2.ValueInDegrees(), 180);
    BOOST_CHECK_EQUAL(a3.ValueInDegrees(), 360);
    BOOST_CHECK_EQUAL(a1.Value(), float(oglplus::math::HalfPi()));
    BOOST_CHECK_EQUAL(a2.Value(), float(oglplus::math::Pi()));
    BOOST_CHECK_EQUAL(a3.Value(), float(oglplus::math::TwoPi()));

    Anglef a4 = oglplus::RightAngles(1.0f);
    Anglef a5 = oglplus::FullCircles(1.0f);

    BOOST_CHECK_EQUAL(a1.ValueInDegrees(), a4.ValueInDegrees());
    BOOST_CHECK_EQUAL(a3.ValueInDegrees(), a5.ValueInDegrees());
}

BOOST_AUTO_TEST_CASE(Angle_cmp) {
    using Anglef = oglplus::Angle<float>;

    Anglef a1 = Anglef::Radians(oglplus::math::HalfPi());
    Anglef a2 = Anglef::Radians(oglplus::math::TwoPi());

    Anglef a3 = oglplus::RightAngles(1.0f);
    Anglef a4 = oglplus::FullCircles(1.0f);

    BOOST_CHECK(a1 == a1);
    BOOST_CHECK(a2 == a2);

    BOOST_CHECK(a1 != a2);
    BOOST_CHECK(a3 != a4);

    BOOST_CHECK(a1 == a3);
    BOOST_CHECK(a2 == a4);
}

BOOST_AUTO_TEST_CASE(Angle_addition) {
    using Angled = oglplus::Angle<double>;

    Angled a0;
    Angled a1 = Angled::Radians(oglplus::math::HalfPi());
    Angled a2 = Angled::Radians(oglplus::math::Pi());
    Angled a3 = Angled::Radians(oglplus::math::TwoPi());

    BOOST_CHECK((a0 + a0) == a0);
    BOOST_CHECK((a0 + a1) == a1);
    BOOST_CHECK((a1 + a1) == a2);
    BOOST_CHECK((a2 + a2) == a3);
    BOOST_CHECK((a1 + a2 + a1) == a3);
    BOOST_CHECK((a1 + a1 + a1 + a1) == a3);
}

BOOST_AUTO_TEST_CASE(Angle_subtraction) {
    using Angled = oglplus::Angle<double>;

    Angled a0;
    Angled a1 = Angled::Radians(oglplus::math::HalfPi());
    Angled a2 = Angled::Radians(oglplus::math::Pi());
    Angled a3 = Angled::Radians(oglplus::math::TwoPi());

    BOOST_CHECK((a0 - a0) == a0);
    BOOST_CHECK((a1 - a1) == a0);
    BOOST_CHECK((a1 - a0) == a1);
    BOOST_CHECK((a0 - a1) == -a1);
    BOOST_CHECK((a2 - a1) == a1);
    BOOST_CHECK((-a1 + a2) == a1);
    BOOST_CHECK((a3 - a2 - a1) == a1);
    BOOST_CHECK((a3 - a2 - a1 - a1) == a0);
    BOOST_CHECK((-a1 - a2 + a3) == a1);
    BOOST_CHECK((-a1 - a1 - a1 - a1) == -a3);
    BOOST_CHECK(-(-a1 - a1 - a2) == a3);
}

BOOST_AUTO_TEST_CASE(Angle_multiplication) {
    using Angled = oglplus::Angle<double>;

    Angled a0;
    Angled a1 = Angled::Radians(oglplus::math::HalfPi());
    Angled a2 = Angled::Radians(oglplus::math::Pi());
    Angled a3 = Angled::Radians(oglplus::math::TwoPi());

    BOOST_CHECK((2 * a0) == a0);
    BOOST_CHECK((0 * a1) == a0);
    BOOST_CHECK((2 * a1) == a2);
    BOOST_CHECK((2 * a2) == a3);
    BOOST_CHECK(a1 == (a2 * 0.5));
    BOOST_CHECK((4 * a1) == (2 * a2));
    BOOST_CHECK((2.1 * a1) == (a1 * 2.1));
    BOOST_CHECK((3.0 * a1) == (a2 * 1.5));
    BOOST_CHECK((2.4 * a1) == (a2 * 1.2));
    BOOST_CHECK((4.4 * a1) == (a3 * 1.1));
    BOOST_CHECK((oglplus::math::TwoPi() * a1) == (a2 * oglplus::math::Pi()));
}

BOOST_AUTO_TEST_CASE(Angle_division) {
    using Angled = oglplus::Angle<double>;

    Angled a0;
    Angled a1 = Angled::Radians(oglplus::math::HalfPi());
    Angled a2 = Angled::Radians(oglplus::math::Pi());
    Angled a3 = Angled::Radians(oglplus::math::TwoPi());

    BOOST_CHECK(a0 == (a0 / 2));
    BOOST_CHECK(a1 == (a2 / 2));
    BOOST_CHECK(a2 == (a3 / 2));
    BOOST_CHECK(a1 == (a3 / 4));
    BOOST_CHECK((a1 / 3) == (a2 / 6));
    BOOST_CHECK((a1 / 3) == (a3 / 12));
    BOOST_CHECK((a1 / oglplus::math::Pi()) == (a2 / oglplus::math::TwoPi()));
}

BOOST_AUTO_TEST_CASE(Angle_arithmetic) {
    using Angled = oglplus::Angle<double>;

    Angled a0;
    Angled a1 = Angled::Radians(oglplus::math::HalfPi());
    Angled a2 = Angled::Radians(oglplus::math::Pi());
    Angled a3 = Angled::Radians(oglplus::math::TwoPi());

    BOOST_CHECK((a1 + a1 + a1) == (3.0 * a2) / 2.0);
    BOOST_CHECK((a1 + a0) == (a1 - a0));
    BOOST_CHECK(2.0 * (-a1 - a2 + a3) == a3 / 2.0);
    BOOST_CHECK((9 * a1 - 4 * a2) * 3.0 + a1 == a3);
}

BOOST_AUTO_TEST_CASE(Angle_sin_cos) {
    using Angled = oglplus::Angle<double>;

    Angled a0;
    Angled a1 = Angled::Radians(oglplus::math::HalfPi());
    Angled a2 = Angled::Radians(oglplus::math::Pi());
    Angled a3 = Angled::Radians(oglplus::math::TwoPi());
    double eps = 1e-9;

    BOOST_CHECK_EQUAL(Sin(a0), 0.0);
    BOOST_CHECK_EQUAL(Cos(a0), 1.0);

    BOOST_CHECK_CLOSE(Sin(a1), 1.0, eps);
    BOOST_CHECK_CLOSE(Sin(a2) + 1.0, 1.0, eps);
    BOOST_CHECK_CLOSE(Sin(a2) + 1.0, Sin(a0) + 1.0, eps);
    BOOST_CHECK_CLOSE(Sin(a2 + a1), -1.0, eps);

    BOOST_CHECK_CLOSE(Cos(a1) + 1.0, 1.0, eps);
    BOOST_CHECK_CLOSE(Cos(a2), -1.0, eps);
    BOOST_CHECK_CLOSE(Cos(a2 + a1) + 1.0, 1.0, eps);

    BOOST_CHECK_CLOSE(Sin(a1), Sin(100000 * a3 + a1), eps);
    BOOST_CHECK_CLOSE(Cos(a0), Cos(100000 * a3 + a0), eps);

    BOOST_CHECK(Sin(a2 + a1) == Sin(a3 - a1));
    BOOST_CHECK(Cos(a2 + a1) == Cos(a3 - a1));
}

BOOST_AUTO_TEST_CASE(Angle_sin_cos_2) {
    using Angled = oglplus::Angle<double>;
    Angled a3 = Angled::Radians(oglplus::math::TwoPi());

    double eps = 1e-8;
    double d = 1.6181;

    for(unsigned i = 0; i != 1000; ++i) {
        Angled a = Angled::Radians(i * i * d);
        BOOST_CHECK_CLOSE(Sin(a) + 2.0, Sin(a + a3 * i) + 2.0, eps);
        BOOST_CHECK_CLOSE(Cos(a) + 2.0, Cos(a + a3 * i) + 2.0, eps);
    }
}

BOOST_AUTO_TEST_CASE(Angle_tan) {
    using Angled = oglplus::Angle<double>;

    double eps = 1e-9;
    double d = 1.6181;

    for(unsigned i = 0; i != 1000; ++i) {
        Angled a = Angled::Radians(i * i * d);
        BOOST_CHECK_CLOSE(Sin(a) / Cos(a) + 2.0, Tan(a) + 2.0, eps);
    }
}

BOOST_AUTO_TEST_CASE(Angle_arc) {
    using oglplus::ArcCos;
    using oglplus::ArcSin;

    using Angled = oglplus::Angle<double>;

    double eps = 2;

    for(unsigned i = 1; i != 1000; ++i) {
        Angled a = Angled::Radians(oglplus::math::HalfPi() / i);
        BOOST_CHECK_CLOSE(a.Value(), ArcSin(Sin(a)).Value(), eps);
        BOOST_CHECK_CLOSE(a.Value(), ArcCos(Cos(a)).Value(), eps);
    }
}

BOOST_AUTO_TEST_SUITE_END()


#define BOOST_TEST_NO_LIB
#include <boost/test/auto_unit_test.hpp>

#include "coconut/pulp/math/Vector.hpp"

using namespace coconut;
using namespace coconut::pulp;
using namespace coconut::pulp::math;

namespace /* anonymous */ {

BOOST_AUTO_TEST_SUITE(PulpTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathTestSuite);
BOOST_AUTO_TEST_SUITE(PulpMathVectorTestSuite);

BOOST_AUTO_TEST_CASE(VectorConstructionAndElementAccessWorks) {
	const auto oneTwoThreeFour = Vec4(1.0f, 2.0f, 3.0f, 4.0f);
	auto oneTwoThenThree = Vector<int, 2>{ 1, 2 };

	BOOST_CHECK_EQUAL(oneTwoThreeFour.x(), 1.0f);
	BOOST_CHECK_EQUAL(oneTwoThreeFour.y(), 2.0f);
	BOOST_CHECK_EQUAL(oneTwoThreeFour.z(), 3.0f);
	BOOST_CHECK_EQUAL(oneTwoThreeFour.w(), 4.0f);

	BOOST_CHECK_EQUAL(oneTwoThenThree.x(), 1);
	BOOST_CHECK_EQUAL(oneTwoThenThree.y(), 2);
	oneTwoThenThree.y() = 4;
	BOOST_CHECK_EQUAL(oneTwoThenThree.y(), 4);
}

BOOST_AUTO_TEST_CASE(DefaultConstructorYieldsZeros) {
	const auto zero = Vec2();
	BOOST_CHECK_EQUAL(zero, Vec2(0.0f, 0.0f));
}

BOOST_AUTO_TEST_CASE(HigherDimensionVectorsAreConstructibleFromLower) {
	const auto oneTwoThree = Vector<std::int32_t, 3>(1, 2, 3);
	const auto oneTwoThreeFour = Vector<std::int32_t, 4>(oneTwoThree, 4);
	const auto oneTwoThreeFourFive = Vector<std::int64_t, 5>(oneTwoThree, 4, 5);

	const auto expected1234 = Vector<std::int32_t, 4>(1, 2, 3, 4);
	const auto expected12345 = Vector<std::int64_t, 5>(1, 2, 3, 4, 5);

	BOOST_CHECK_EQUAL(oneTwoThreeFour, expected1234);
	BOOST_CHECK_EQUAL(oneTwoThreeFourFive, expected12345);
}

BOOST_AUTO_TEST_CASE(VectorsAreEqualityComparable) {
	const auto iOne = Vector<int, 1>(1);
	const auto iOne2 = Vector<int, 1>(1);
	const auto iTwo = Vector<int, 1>(2);

	BOOST_CHECK_EQUAL(iOne, iOne2);
	BOOST_CHECK_NE(iOne, iTwo);

	const auto fOneTwo = Vec2(1.0f, 2.0f);
	const auto fOneAlmostTwo = Vec2(1.0f, 2.0f - 0.0000001f);
	const auto fOneNotTwo = Vec2(1.0f, 2.0f - 0.001f);

	BOOST_CHECK_EQUAL(fOneTwo, fOneAlmostTwo);
	BOOST_CHECK_NE(fOneTwo, fOneNotTwo);
}

BOOST_AUTO_TEST_CASE(VectorsAreAdditive) {
	auto lhs = Vec3(1.0f, 1.0f, 1.0f);
	const auto rhs = Vec3(0.0f, 1.0f, 2.0f);
	const auto sum = Vec3(1.0f, 2.0f, 3.0f);

	BOOST_CHECK_EQUAL(lhs + rhs, sum);

	lhs += rhs;
	BOOST_CHECK_EQUAL(lhs, sum);

	lhs -= rhs;
	BOOST_CHECK_EQUAL(lhs, sum - rhs);
}

BOOST_AUTO_TEST_CASE(VectorsAreNegatable) {
	auto vector = Vec2(4.5f, 6.7f);
	auto negated = Vec2(-4.5f, -6.7f);

	BOOST_CHECK_EQUAL(-vector, negated);
}

BOOST_AUTO_TEST_CASE(VectorsAreScalarMultiplicative) {
	auto vector = Vec2(0.5f, 1.0f);
	const auto product = Vec2(1.0f, 2.0f);

	BOOST_CHECK_EQUAL(vector * 2, product);

	vector *= 2.0f;
	BOOST_CHECK_EQUAL(vector, product);

	vector /= 2.0f;
	BOOST_CHECK_EQUAL(vector, product / 2.0f);
}

BOOST_AUTO_TEST_CASE(DotProducesDotProduct) {
	const auto lhs = Vec2(1.0f, 25.0f);
	const auto rhs = Vec2(0.5f, 0.1f);

	BOOST_CHECK_EQUAL(dot(lhs, rhs), 3.0f);
}

BOOST_AUTO_TEST_CASE(CrossProducesCrossProduct) {
	auto lhs = Vec3(0.123f, 4.5f, -3.14f);
	const auto rhs = Vec3(0.0f, -0.1f, 5.001f);
	const auto product = Vec3(22.1905f, -0.615123f, -0.0123f);

	BOOST_CHECK_EQUAL(cross(lhs, rhs), product);
}

BOOST_AUTO_TEST_CASE(LengthReturnsVectorLength) {
	const auto vec = Vec2(3.12f, 0.14f);

	BOOST_CHECK_CLOSE(vec.length(), 3.12313944613f, 0.01f);
	BOOST_CHECK_CLOSE(vec.lengthSq(), 9.754f, 0.01f);
}

BOOST_AUTO_TEST_CASE(NormalisedReturnsNormalised) {
	auto vec = Vec2(3.12f, 0.14f);
	const auto normalised = Vec2(0.998994778f, 0.044826688f);

	BOOST_CHECK_EQUAL(vec.normalised(), normalised);

	vec.normalise();
	BOOST_CHECK_EQUAL(vec, normalised);

	BOOST_CHECK_CLOSE(vec.lengthSq(), 1.0f, 0.01f);
}

BOOST_AUTO_TEST_SUITE_END(/* PulpMathVectorTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpMathTestSuite */);
BOOST_AUTO_TEST_SUITE_END(/* PulpTestSuite */);

} // namespace anonymous