#include <gtest/gtest.h>
#include <boost/lexical_cast.hpp>

#include "base/eigen2hdf.hpp"
#include "fft/fft2.hpp"
#include "fft/fft2_r2c.hpp"
#include "ridgelet/construction/translation_grid.hpp"
#include "ridgelet/ridgelet_cell_array.hpp"
#include "ridgelet/ridgelet_frame.hpp"
#include "ridgelet/rt.hpp"

using namespace std;

/**
 *  @brief test rt -> irt
 *
 *  performs tests for PlannerR2COD
 *
 *  @param param
 *  @return return type
 */
TEST(base, rt)
{
  typedef RT<double, RidgeletFrame, FFTr2c<PlannerR2COD> > rt_t;
  typedef typename rt_t::numeric_t numeric_t;
  typedef typename rt_t::complex_array_t complex_array_t;
  typedef typename rt_t::fft_t fft_t;
  typedef typename rt_t::rt_coeff_t rt_coeff_t;
  fft_t fft;
  double tol = 1e-15;

  int J = 7;

  unsigned int rho_x = 1;
  unsigned int rho_y = 1;

  for (int j = 0; j < J; ++j) {
    RidgeletFrame rf(J, J, rho_x, rho_y);
    unsigned int Nx = rf.Nx();
    unsigned int Ny = rf.Ny();
    rt_t rt(rf);

    typedef Eigen::Array<numeric_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> array_t;
    array_t R(Nx, Ny);
    R.setRandom();

    std::vector<rt_coeff_t> rt_coeffs(rf.size());

    complex_array_t Fh(Nx, Ny);
    fft.ft(Fh, R);
    rt.rt(rt_coeffs, Fh);

    // check that tgrid_dim is correct
    auto& lambdas = rf.lambdas();
    for (unsigned int i = 0; i < lambdas.size(); ++i) {
      auto tt = tgrid_dim(lambdas[i], rf);
      int rows = rt_coeffs[i].rows();
      int cols = rt_coeffs[i].cols();

      EXPECT_EQ(rows * cols, std::get<0>(tt) * std::get<1>(tt)) << lambdas[i];
      // EXPECT_EQ(rows, std::get<0>(tt)) << lambdas[i];
      // EXPECT_EQ(cols, std::get<1>(tt)) << lambdas[i];
    }

    complex_array_t Fh2(Nx, Ny);
    rt.irt(Fh2, rt_coeffs);

    // check that is invertible
    array_t DIFF = (ftcut(Fh2, Nx / 2, Ny / 2) - ftcut(Fh, Nx / 2, Ny / 2)).abs();
    double dmax = DIFF.maxCoeff();
    EXPECT_TRUE(dmax < tol) << dmax;
  }
}

/**
 *  @brief test rt -> irt
 *
 *  performs tests for PlannerR2COD
 *
 *  @param param
 *  @return return type
 */
TEST(base, rt_planned)
{
  typedef RT<double, RidgeletFrame, FFTr2c<PlannerR2C> > rt_t;
  typedef typename rt_t::numeric_t numeric_t;
  typedef typename rt_t::complex_array_t complex_array_t;
  typedef typename rt_t::fft_t fft_t;
  typedef typename rt_t::rt_coeff_t rt_coeff_t;
  fft_t fft;
  double tol = 1e-15;

  int J = 7;

  unsigned int rho_x = 1;
  unsigned int rho_y = 1;

  for (int j = 0; j < J; ++j) {
    RidgeletFrame rf(J, J, rho_x, rho_y);
    unsigned int Nx = rf.Nx();
    unsigned int Ny = rf.Ny();
    rt_t rt(rf);
    init_fftw(fft, FFTW_MEASURE, rf);

    typedef Eigen::Array<numeric_t, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> array_t;
    array_t R(Nx, Ny);
    R.setRandom();

    std::vector<rt_coeff_t> rt_coeffs(rf.size());

    complex_array_t Fh(Nx, Ny);
    fft.ft(Fh, R);
    rt.rt(rt_coeffs, Fh);

    // check that tgrid_dim is correct
    auto& lambdas = rf.lambdas();
    for (unsigned int i = 0; i < lambdas.size(); ++i) {
      auto tt = tgrid_dim(lambdas[i], rf);
      int rows = rt_coeffs[i].rows();
      int cols = rt_coeffs[i].cols();

      EXPECT_EQ(rows * cols, std::get<0>(tt) * std::get<1>(tt)) << lambdas[i];
      // EXPECT_EQ(rows, std::get<0>(tt)) << lambdas[i];
      // EXPECT_EQ(cols, std::get<1>(tt)) << lambdas[i];
    }

    complex_array_t Fh2(Nx, Ny);
    rt.irt(Fh2, rt_coeffs);

    // check that is invertible
    array_t DIFF = (ftcut(Fh2, Nx / 2, Ny / 2) - ftcut(Fh, Nx / 2, Ny / 2)).abs();
    double dmax = DIFF.maxCoeff();
    EXPECT_TRUE(dmax < tol) << dmax;
  }
}

/**
 *  @brief check that rt transform is invertible if the Fourier coefficients are padded by zeros.
 *  (the zero-padding is done to have invertibility on the entire grid)
 *  Detailed description
 *
 *  @param param
 *  @return return type
 */
TEST(base, rt_padded)
{
  typedef RT<double, RidgeletFrame, FFTr2c<PlannerR2COD> > rt_t;
  typedef typename rt_t::numeric_t numeric_t;
  typedef typename rt_t::complex_array_t complex_array_t;
  typedef typename rt_t::fft_t fft_t;
  typedef typename rt_t::rt_coeff_t rt_coeff_t;
  fft_t fft;
  double tol = 1e-15;

  int J = 6;

  unsigned int rho_x = 1;
  unsigned int rho_y = 1;

  for (int j = 0; j < J; ++j) {
    RidgeletFrame rf(J, J, rho_x, rho_y);
    unsigned int Nx = rf.Nx();
    unsigned int Ny = rf.Ny();
    unsigned int nx = Nx / 2;
    unsigned int ny = Ny / 2;
    rt_t rt(rf);

    typedef Eigen::Array<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> array_t;
    array_t R(nx, ny);
    R.setRandom();

    std::vector<rt_coeff_t> rt_coeffs(rf.size());

    complex_array_t Fh0(nx, ny);
    fft.ft(Fh0, R);
    // set lowest frequency to zero (e.g. make sure that the Fourier series is
    // real-valued also on the fine grid)
    hf_zero(Fh0);

    complex_array_t Fh(Nx, Ny);
    Fh.setZero();
    ftcut(Fh, nx, ny) = Fh0;

    rt.rt(rt_coeffs, Fh);

    // check that tgrid_dim is correct
    auto& lambdas = rf.lambdas();
    for (unsigned int i = 0; i < lambdas.size(); ++i) {
      auto tt = tgrid_dim(lambdas[i], rf);
      int rows = rt_coeffs[i].rows();
      int cols = rt_coeffs[i].cols();

      EXPECT_EQ(rows * cols, std::get<0>(tt) * std::get<1>(tt)) << lambdas[i];

      // EXPECT_EQ(rows, std::get<0>(tt)) << lambdas[i];
      // EXPECT_EQ(cols, std::get<1>(tt)) << lambdas[i];
    }

    complex_array_t Fhp(Nx, Ny);
    rt.irt(Fhp, rt_coeffs);

    complex_array_t Fhp0(nx, ny);
    Fhp0 = ftcut(Fhp, nx, ny);

    std::string fname = "base_rt_padded" + boost::lexical_cast<std::string>(j) + ".h5";
    hid_t h5f = H5Fcreate(fname.c_str(), H5F_ACC_TRUNC, H5P_DEFAULT, H5P_DEFAULT);
    eigen2hdf::save(h5f, "Fh0", Fh0);
    eigen2hdf::save(h5f, "Fh", Fh);
    eigen2hdf::save(h5f, "Fhp", Fhp);
    H5Fclose(h5f);

    // check that is invertible
    array_t DIFF = (Fhp0 - Fh0).abs();
    double dmax = DIFF.maxCoeff();
    EXPECT_TRUE(dmax < tol) << dmax;
  }
}

// todo check irt, when ft coeffs are padded


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          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 NT2_TOOLBOX_HYPERBOLIC_FUNCTIONS_SCALAR_SINHCOSH_HPP_INCLUDED
#define NT2_TOOLBOX_HYPERBOLIC_FUNCTIONS_SCALAR_SINHCOSH_HPP_INCLUDED

#include <nt2/toolbox/hyperbolic/functions/sinhcosh.hpp>
#include <nt2/include/functions/scalar/tofloat.hpp>
#include <nt2/include/functions/scalar/expm1.hpp>
#include <nt2/include/functions/scalar/negif.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/oneplus.hpp>
#include <nt2/include/functions/scalar/abs.hpp>
#include <nt2/include/functions/scalar/is_negative.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/half.hpp>
#include <nt2/sdk/meta/as_logical.hpp>
#include <nt2/sdk/meta/as_floating.hpp>
#include <boost/fusion/tuple.hpp>

namespace nt2 { namespace ext
{
  NT2_FUNCTOR_IMPLEMENTATION(  nt2::tag::sinhcosh_, tag::cpu_,(A0)(A1)
                               , ((scalar_<arithmetic_<A0> >))
                                 ((scalar_<floating_<A1> >))
                                 ((scalar_<floating_<A1> >))
                             )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a0,A1 & a1,A1 & a2) const
    {
      a2 =  nt2::abs(a0);
      a1 = a0;
      if (a2 == Inf<A1>()) return 0;
      const A1 u = expm1(a2);
      const A1 up1 = oneplus(u);
      const A1 tmp =u/up1;
      a1 = negif(is_negative(a0), Half<A1>()*tmp*(oneplus(up1)));
      a2 = oneplus(Half<A1>()*tmp*u);
      return 0;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sinhcosh_, tag::cpu_,(A0)(A1),
                             ((scalar_ < arithmetic_<A0> > ))
                             ((scalar_ < floating_<A1> > ))
                             )
    {
    typedef A1 result_type;
    inline result_type operator()(A0 const& a0,A1 & a2) const
    {
      A1 a1;
      sinhcosh(tofloat(a0),a1, a2);
      return a1;
    }
  };

  NT2_FUNCTOR_IMPLEMENTATION(nt2::tag::sinhcosh_, tag::cpu_,
                         (A0),
                         ((scalar_<arithmetic_<A0> >))
                        )
  {
      typedef typename meta::as_floating<A0>::type  rtype;
      typedef boost::fusion::tuple<rtype, rtype> result_type;

    NT2_FUNCTOR_CALL(1)
    {
      result_type res;
      sinhcosh(tofloat(a0), boost::fusion::at_c<0>(res),
                 boost::fusion::at_c<1>(res));
      return res;
    }
  };
} }
#endif


//
// Created by Hamza El-Kebir on 5/9/21.
//

#ifndef LODESTAR_DISCRETELQE_HPP
#define LODESTAR_DISCRETELQE_HPP

#include "Lodestar/synthesis/AlgebraicRiccatiEquation.hpp"
#include <Eigen/QR>
#include <vector>

namespace ls {
    namespace filter {
        class DiscreteLQE {
            static Eigen::MatrixXd
            infiniteHorizon(const Eigen::MatrixXd &A,
                            const Eigen::MatrixXd &B,
                            const Eigen::MatrixXd &Q,
                            const Eigen::MatrixXd &C,
                            const Eigen::MatrixXd &R);

            static Eigen::MatrixXd
            infiniteHorizon(const systems::StateSpace<> &sys,
                            const Eigen::MatrixXd &Q,
                            const Eigen::MatrixXd &R);

            static Eigen::MatrixXd
            finiteHorizon(const Eigen::MatrixXd &A,
                          const Eigen::MatrixXd &B,
                          const Eigen::MatrixXd &C,
                          const Eigen::MatrixXd &Q,
                          const Eigen::MatrixXd &R,
                          unsigned int N = 5);

            static Eigen::MatrixXd
            finiteHorizon(const systems::StateSpace<> &sys,
                          const Eigen::MatrixXd &Q,
                          const Eigen::MatrixXd &R,
                          unsigned int N = 5);
        };
    }
}

#endif //LODESTAR_DISCRETELQE_HPP


#include <cstdlib>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>
#include <Eigen/Geometry>
#include <Inventor/SoDB.h>
#include <Inventor/SoOutput.h>
#include <Inventor/actions/SoWriteAction.h>
#include <Inventor/VRMLnodes/SoVRMLInline.h>
#include <Inventor/VRMLnodes/SoVRMLTransform.h>

int
main(int argc, char** argv)
{
	if (argc != 3)
	{
		std::cerr << "Usage: csv2wrl [input.csv] [output.wrl]" << std::endl;
		return EXIT_FAILURE;
	}

	SoDB::init();

	SoVRMLTransform* root = new SoVRMLTransform();
	root->ref();

	std::fstream input;
	input.open(argv[1]);

	for (std::string line; std::getline(input, line);)
	{
		std::istringstream stream(line);

		std::string name;
		std::getline(stream, name, ',');

		std::string number;
		std::getline(stream, number, ',');
		double x = std::atof(number.c_str());
		std::getline(stream, number, ',');
		double y = std::atof(number.c_str());
		std::getline(stream, number, ',');
		double z = std::atof(number.c_str());
		std::getline(stream, number, ',');
		double a = std::atof(number.c_str());
		std::getline(stream, number, ',');
		double b = std::atof(number.c_str());
		std::getline(stream, number, ',');
		double c = std::atof(number.c_str());

		Eigen::AngleAxis<double> rotation(
			Eigen::AngleAxis<double>(c * static_cast<double>(M_PI) / 180, Eigen::Vector3d::UnitZ()) *
			Eigen::AngleAxis<double>(b * static_cast<double>(M_PI) / 180, Eigen::Vector3d::UnitY()) *
			Eigen::AngleAxis<double>(a * static_cast<double>(M_PI) / 180, Eigen::Vector3d::UnitX())
		);

		SoVRMLTransform* vrmlTransform = new SoVRMLTransform();
		vrmlTransform->setName(name.c_str());
		vrmlTransform->rotation.setValue(SbVec3f(rotation.axis().x(), rotation.axis().y(), rotation.axis().z()), rotation.angle());
		vrmlTransform->translation.setValue(x / 1000, y / 1000, z / 1000);
		SoVRMLInline* vrmlInline = new SoVRMLInline();
		vrmlInline->url.setValue(std::string(name + ".wrl").c_str());
		vrmlTransform->addChild(vrmlInline);
		root->addChild(vrmlTransform);
	}

	input.close();

	SoOutput output;
	output.openFile(argv[2]);
	output.setHeaderString("#VRML V2.0 utf8");
	SoWriteAction writeAction(&output);
	writeAction.apply(root);
	output.closeFile();

	root->unref();

	return EXIT_SUCCESS;
}


#ifndef HELPERSDNA
#define HELPERSDNA

#include <boost/operators.hpp>
#include <iostream>
#include <ostream>
#include <boost/bimap.hpp>
#include <string>
#include <map>
#include<vector>

using namespace std;

template<class uint>
unsigned degree(uint v){
	uint r = 0;
	while (v >>= 1) {
		r++;
	}
	return r;
}

template<class cf_t>
ostream &operator<<(ostream &stream, const vector<cf_t>& x)
{
	for(unsigned i = 0; i<x.size(); ++i) stream << x[i] << "  ";
	return stream; // must return stream
};

template<class vectt>
unsigned hamming_distance(vectt& v1,vectt& v2){
	assert(v1.size()=v2.size());
	unsigned hd = 0;
	for(unsigned i=0;i<v1.size();++i)
		if(v1[i] != v2[i]) hd++;
	return hd;
}

template<class vectt>
void flipvecdir(vectt& v){
	vectt tmp = v;
	for(unsigned i=0;i<v.size();++i){
		v[i] = tmp[v.size()-1-i];
	}
}

// convert decimal number to a number with another base
// nu: 		an unsigned integer
// base: 	base of the new representation
// size: 	size of the vector required for the new representation
template<class base_t>
vector<base_t> tobase(uint64_t nu, uint64_t base, unsigned size){

	vector<base_t> nn(size,(base_t) 0 );
	if(nu == 0) return nn;

	uint64_t div = nu;
	unsigned i = 0;
	while(div != 0){
		//nn[i] = base_t(div % base); // rest
		assert( i < nn.size() );
		nn[i] = base_t(div % base);
		div = div/base;
		i++;
	}

	return nn;
};

// convert vector of numbers in some basis to decimal number
template<class base_t>
uint64_t frombase(const vector<base_t>& nn,uint64_t base){
	unsigned deg = degree(base);

	assert(deg*nn.size() < 64); // otherwise we have an overflow below

	uint64_t nu = 0;
	uint64_t basepow = 1;
	for(unsigned i=0;i<nn.size();++i){
		uint64_t adf = nn[i];
		nu += ((uint64_t)adf) * basepow;//((unsigned) (adf)) * ((int) pow((double) base,(int) i));
		basepow *= base;
	}
	return nu;
};

template<class GFM,class GFN>
void GFM2GFN(vector<GFM>& from, vector<GFN>& to){
	// choose a and b as smallest numbers such that a*m == b*n
	unsigned a = 1;
	unsigned b = 1;
	while (a*GFM::m != b*GFN::m){
		if(a*GFM::m < b*GFN::m)
			a += 1;
		else
			b += 1;
	}
	while( (from.size() % a) != 0 ) from.push_back(GFM(0));
    to.resize(0);
    // every a entries of `from' are mapped to b entries of `to'
	for(unsigned i=0;i<from.size()/a; ++i){
		// the unsigned char type below is important, otherwise errors occur when converting..
		vector<uint> ind;
		for(typename vector<GFM>::iterator it = from.begin()+i*a; it != from.begin()+i*a+a; ++it){
			ind.push_back(it->el);
		}
		unsigned mm = GFM::m;
		assert(mm < 64);
		uint64_t powmm = 1;
		powmm = powmm << mm;
		uint64_t pownm = 1;
		unsigned nm = GFN::m;
		assert(nm < 64);
		pownm = pownm << nm;
		int64_t	indec = frombase( ind , powmm );
        vector<uint> tmpvec = tobase<uint>(indec, pownm ,b);
		for(uint j : tmpvec)
			to.push_back(GFN(j,0));
		//to.insert(to.begin()+i*b,tmpvec.begin(),tmpvec.end());
	}
};

template<class uint,unsigned m, unsigned n>
void gfm2gfn(vector<uint>& from, vector<uint>& to){
	// choose a and b as smallest numbers such that a*m == b*n
	unsigned a = 1;
	unsigned b = 1;
	while (a*m != b*n){
		if(a*m < b*n)
			a += 1;
		else
			b += 1;
	}
	while((from.size() % a) != 0 ) from.push_back(0);

    to.resize(0);
    // every a entries of `from' are maped to three entries of `to'
	for(unsigned i=0;i<from.size()/a; ++i){
        // the unsigned char type below is important, otherwise errors occur when converting..
		unsigned indec = frombase(vector<unsigned char>(from.begin()+i*a, from.begin()+i*a+a), 1<<m );
        vector<uint> tmpvec = tobase<uint>(indec,1<<n,b);
        to.insert(to.begin()+i*b,tmpvec.begin(),tmpvec.end());
	}
};

// converts each 2 characters in data_char to 3 elements of GF(47), which are appended at
template<class pfe>
void char2pfe(string& data_char, vector<pfe>& data_b47){
    if((data_char.size() % 2) != 0 ) data_char.push_back('\n');

    data_b47.resize(0);
	// convert vector of char to vector with base 47;
    // every 2 char's are coded to three pfe's
    for(unsigned i=0;i<data_char.size()/2; ++i){
        // the unsigned char type below is important, otherwise errors occur when converting..
		unsigned indec = frombase(vector<unsigned char>(data_char.begin()+i*2, data_char.begin()+i*2+2),256);

        vector<pfe> tmpvec = tobase<pfe>(indec,47,3);
        data_b47.insert(data_b47.begin()+i*3,tmpvec.begin(),tmpvec.end());
	}
};

template<class pfe>
void pfe2char(string& data_char, vector<pfe>& data_b47){

    assert(data_b47.size() % 3  == 0);

    data_char.resize(2*(data_char.size()/3) );
    // convert vector of char to vector with base 47;
    // every 2 char's are coded to three pfe's
    for(unsigned i=0;i<data_b47.size()/3; ++i){
        unsigned indec = frombase(vector<pfe>(data_b47.begin()+i*3, data_b47.begin()+i*3+3),47);

        vector<char> tmpvec = tobase<char>(indec,256,2);
        data_char.insert(data_char.begin()+i*2,tmpvec.begin(),tmpvec.end());
    }

};

/*
maps each element of GF2M to a string with letters {A,C,G,T}
*/

template<class GF>
class DNAmapGF{
public:
	DNAmapGF(){assert(GF::m % 2 == 0);};
	// codeword to DNA fragment
	void cw2frag(const vector<GF>& cw, string& frag ){
		frag.resize(0);
		for(GF gf: cw){
			for(unsigned i=0; i<GF::m; i+=2){
				switch( (gf.el >> i) & 3 ) { // shift by two, and then check the bits..
    				case 0 : frag.append("A");
							 break;
					case 1 : frag.append("C");
							 break;
					case 2 : frag.append("G");
							 break;
					case 3 : frag.append("T");
							 break;
				}
			}
		}
	}

	// DNA fragment to codeword
	void frag2cw(vector<GF>& cw, const string& frag ){
		assert( (frag.size() % (GF::m/2)) == 0 );
		cw.resize( frag.size() / (GF::m/2) );
		for(unsigned i=0;i<cw.size();++i){
			typename GF::uint el = 0;
			for(unsigned j=0; j<GF::m/2; j++){
				switch(frag[i*GF::m/2 + j]){
    				case 'A' :  // add zero..
							 break;
					case 'C' : el += ((typename GF::uint) 1) << 2*j ;
							 break;
					case 'G' : el += ((typename GF::uint) 2) << 2*j ;
							 break;
					case 'T' : el += ((typename GF::uint) 3) << 2*j ;
							 break;
				}
			}
			cw[i] = GF(el,0);
		}
	}
};

/*
maps each element of GF(47) to a string with letters {A,C,G,T}
*/

template<class PFE>
class DNAmap{
private:
	//typedef boost::bimap< PFE , std::string > mapdna_type;
	//mapdna_type mapdna;

	map<PFE,string> pfetostr;
	map<string,PFE> strtopfe;

public:
	DNAmap(){
		//initialize map
		unsigned prime = 47;

		char nucl[] = "ACGT"; // nucleotides

		vector<string> allpos(4*4*4);
		for(unsigned i=0;i<allpos.size();++i){
			char cur[] = "AAA";
			cur[0] = nucl[i % 4];
			cur[1] = nucl[(i/4) % 4];
			cur[2] = nucl[(i/16) % 4];
			allpos[i] = string(cur);
		}

		unsigned j = 0;
		for(unsigned i=0;i<prime;++i){
			while( allpos[j][1] == allpos[j][2]) j++;
			//mapdna.insert( typename mapdna_type::value_type(PFE(i), allpos[j] ));
			pfetostr[PFE(i)] = allpos[j];
			strtopfe[allpos[j]] = PFE(i);

			j++;
		}
		//print_map(mapdna.right, " ", cout);
	}

	// codeword to DNA fragment
	void cw2frag(const vector<PFE>& cw, string& frag ){
		frag.resize(0);
		for(unsigned i=0;i<cw.size();++i){
			frag.append(pfetostr[cw[i]]); //mapdna.left.at(cw[i]);
		}
	}

	// DNA fragment to codeword
	void frag2cw(vector<PFE>& cw, const string& frag ){
		assert( (frag.size() % 3) == 0   );
		cw.resize(frag.size()/3);
		for(unsigned i=0;i<cw.size();++i){

			//char charartmp[] = "AAA"; // nucleotides
			//charartmp[0] = frag[i*3];
			//charartmp[1] = frag[i*3+1];
			//charartmp[2] = frag[i*3+2];
			//string tmp(charartmp);
			string tmp = string(frag.begin()+i*3, frag.begin()+i*3+3  );

			//print_map(mapdna.right, " ", cout);

			//cout <<  mapdna.right.at(tmp )  << endl;
			//cw[i] = PFE( mapdna.right.at(tmp ) );
			cw[i] = strtopfe[tmp];
			//cout << cw[i] << endl;
		}
	}
};

void fliplett(string& frag){
    for(unsigned i=0;i<frag.size();++i){
        switch(frag[i]){
            case 'T': frag[i] = 'A';
                break;
            case 'A': frag[i] = 'T';
                break;
            case 'G': frag[i] = 'C';
                break;
            case 'C': frag[i] = 'G';
                break;
        }
    }
}

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013   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_META_PREV_POWER_OF_2_HPP_INCLUDED
#define BOOST_SIMD_META_PREV_POWER_OF_2_HPP_INCLUDED

/*!
  @file
  @brief Defines and implements prev_power_of_2 and prev_power_of_2_c
**/

#include <boost/mpl/integral_c.hpp>
#include <boost/mpl/size_t.hpp>
#include <cstddef>

namespace boost { namespace simd {  namespace details
{
  // Adaptation of :
  // http://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2
  template<std::size_t N> struct prev_power_of_2_impl
  {
    BOOST_STATIC_CONSTANT(std::size_t, x0    = N               );
    BOOST_STATIC_CONSTANT(std::size_t, x1    = x0 | (x0 >>  1) );
    BOOST_STATIC_CONSTANT(std::size_t, x2    = x1 | (x1 >>  2) );
    BOOST_STATIC_CONSTANT(std::size_t, x3    = x2 | (x2 >>  4) );
    BOOST_STATIC_CONSTANT(std::size_t, x4    = x3 | (x3 >>  8) );
    BOOST_STATIC_CONSTANT(std::size_t, x5    = x4 | (x4 >> 16) );
    BOOST_STATIC_CONSTANT(std::size_t, value = (x5 >> 1) + 1   );
  };

  // Requried for MSVC
  template<> struct prev_power_of_2_impl<0>
  {
    BOOST_STATIC_CONSTANT(std::size_t, value = 0 );
  };
} } }

namespace boost { namespace simd {  namespace meta
{
  /*!
    @brief Evaluates previous power of two

    Computes the power of two lesser or equal to any given integral value @c N.

    @par Semantic:
    For any given integral value @c N:

    @code
    typedef prev_power_of_2_c<N>::type r;
    @endcode

    is equivalent to:

    @code
    typedef mpl::size_t<M> r;
    @endcode

    Where @c M is lesser or equal to N and so that it exists a given @c P so
    that @c M is equal to two at the power of @c P.

    @usage{meta/prev_power_of_2_c.cpp}

    @tparam N Integral constant to downgrade
  **/
  template<std::size_t N> struct  prev_power_of_2_c
#if !defined(DOXYGEN_ONLY)
        : boost::mpl::size_t<details::prev_power_of_2_impl<N>::value>
#endif
  {};

  /*!
    @brief Evaluates previous power of two

    Computes the power of two lesser or equal to any given @mplint @c N.

    @par Semantic:
    For any given @mplint @c N:

    @code
    typedef prev_power_of_2<N>::type r;
    @endcode

    is equivalent to:

    @code
    typedef prev_power_of_2<N::value>::type r;
    @endcode

    @par Models:

    @metafunction

    @usage{meta/prev_power_of_2.cpp}

    @tparam N @mplint to downgrade
  **/
  template<typename N> struct  prev_power_of_2
#if !defined(DOXYGEN_ONLY)
        : boost::mpl::integral_c< typename N::value_type
                                , details::prev_power_of_2_impl<N::value>::value
                                >
#endif
  {};
} } }

#endif


#include "main.h"

#include <Eigen/CXX11/Tensor>

using Eigen::Tensor;

static void test_single_voxel_patch()
{
  Tensor<float, 5> tensor(4,2,3,5,7);
  tensor.setRandom();
  Tensor<float, 5, RowMajor> tensor_row_major = tensor.swap_layout();

  Tensor<float, 6> single_voxel_patch;
  single_voxel_patch = tensor.extract_volume_patches(1, 1, 1);
  VERIFY_IS_EQUAL(single_voxel_patch.dimension(0), 4);
  VERIFY_IS_EQUAL(single_voxel_patch.dimension(1), 1);
  VERIFY_IS_EQUAL(single_voxel_patch.dimension(2), 1);
  VERIFY_IS_EQUAL(single_voxel_patch.dimension(3), 1);
  VERIFY_IS_EQUAL(single_voxel_patch.dimension(4), 2 * 3 * 5);
  VERIFY_IS_EQUAL(single_voxel_patch.dimension(5), 7);

  Tensor<float, 6, RowMajor> single_voxel_patch_row_major;
  single_voxel_patch_row_major = tensor_row_major.extract_volume_patches(1, 1, 1);
  VERIFY_IS_EQUAL(single_voxel_patch_row_major.dimension(0), 7);
  VERIFY_IS_EQUAL(single_voxel_patch_row_major.dimension(1), 2 * 3 * 5);
  VERIFY_IS_EQUAL(single_voxel_patch_row_major.dimension(2), 1);
  VERIFY_IS_EQUAL(single_voxel_patch_row_major.dimension(3), 1);
  VERIFY_IS_EQUAL(single_voxel_patch_row_major.dimension(4), 1);
  VERIFY_IS_EQUAL(single_voxel_patch_row_major.dimension(5), 4);

  for (int i = 0; i < tensor.size(); ++i) {
    VERIFY_IS_EQUAL(tensor.data()[i], single_voxel_patch.data()[i]);
    VERIFY_IS_EQUAL(tensor_row_major.data()[i], single_voxel_patch_row_major.data()[i]);
    VERIFY_IS_EQUAL(tensor.data()[i], tensor_row_major.data()[i]);
  }
}

static void test_entire_volume_patch()
{
  const int depth = 4;
  const int patch_z = 2;
  const int patch_y = 3;
  const int patch_x = 5;
  const int batch = 7;

  Tensor<float, 5> tensor(depth, patch_z, patch_y, patch_x, batch);
  tensor.setRandom();
  Tensor<float, 5, RowMajor> tensor_row_major = tensor.swap_layout();

  Tensor<float, 6> entire_volume_patch;
  entire_volume_patch = tensor.extract_volume_patches(patch_z, patch_y, patch_x);
  VERIFY_IS_EQUAL(entire_volume_patch.dimension(0), depth);
  VERIFY_IS_EQUAL(entire_volume_patch.dimension(1), patch_z);
  VERIFY_IS_EQUAL(entire_volume_patch.dimension(2), patch_y);
  VERIFY_IS_EQUAL(entire_volume_patch.dimension(3), patch_x);
  VERIFY_IS_EQUAL(entire_volume_patch.dimension(4), patch_z * patch_y * patch_x);
  VERIFY_IS_EQUAL(entire_volume_patch.dimension(5), batch);

  Tensor<float, 6, RowMajor> entire_volume_patch_row_major;
  entire_volume_patch_row_major = tensor_row_major.extract_volume_patches(patch_z, patch_y, patch_x);
  VERIFY_IS_EQUAL(entire_volume_patch_row_major.dimension(0), batch);
  VERIFY_IS_EQUAL(entire_volume_patch_row_major.dimension(1), patch_z * patch_y * patch_x);
  VERIFY_IS_EQUAL(entire_volume_patch_row_major.dimension(2), patch_x);
  VERIFY_IS_EQUAL(entire_volume_patch_row_major.dimension(3), patch_y);
  VERIFY_IS_EQUAL(entire_volume_patch_row_major.dimension(4), patch_z);
  VERIFY_IS_EQUAL(entire_volume_patch_row_major.dimension(5), depth);

  const int dz = patch_z - 1;
  const int dy = patch_y - 1;
  const int dx = patch_x - 1;

  const int forward_pad_z = dz - dz / 2;
  const int forward_pad_y = dy - dy / 2;
  const int forward_pad_x = dx - dx / 2;

  for (int pz = 0; pz < patch_z; pz++) {
    for (int py = 0; py < patch_y; py++) {
      for (int px = 0; px < patch_x; px++) {
        const int patchId = pz + patch_z * (py + px * patch_y);
        for (int z = 0; z < patch_z; z++) {
          for (int y = 0; y < patch_y; y++) {
            for (int x = 0; x < patch_x; x++) {
              for (int b = 0; b < batch; b++) {
                for (int d = 0; d < depth; d++) {
                  float expected = 0.0f;
                  float expected_row_major = 0.0f;
                  const int eff_z = z - forward_pad_z + pz;
                  const int eff_y = y - forward_pad_y + py;
                  const int eff_x = x - forward_pad_x + px;
                  if (eff_z >= 0 && eff_y >= 0 && eff_x >= 0 &&
                      eff_z < patch_z && eff_y < patch_y && eff_x < patch_x) {
                    expected = tensor(d, eff_z, eff_y, eff_x, b);
                    expected_row_major = tensor_row_major(b, eff_x, eff_y, eff_z, d);
                  }
                  VERIFY_IS_EQUAL(entire_volume_patch(d, z, y, x, patchId, b), expected);
                  VERIFY_IS_EQUAL(entire_volume_patch_row_major(b, patchId, x, y, z, d), expected_row_major);
                }
              }
            }
          }
        }
      }
    }
  }
}

void test_cxx11_tensor_volume_patch()
{
  CALL_SUBTEST(test_single_voxel_patch());
  CALL_SUBTEST(test_entire_volume_patch());
}


#ifndef MPP_HAMILTONIAN_CLASSIC_HAMILTONIAN_HPP
#define MPP_HAMILTONIAN_CLASSIC_HAMILTONIAN_HPP

#include <exception>
#include <sstream>
#include <string>
#include <random>
#include <type_traits>
#include <cmath>
#include <functional>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/assert.hpp>
#include <mpp/config.hpp>
#include <mpp/chains/mcmc_chain.hpp>
#include <mpp/hamiltonian/kinetic_energy_multivar_normal.hpp>

namespace mpp { namespace hamiltonian {

template<class real_scalar_type>
class hmc_sampler
{
public:
    typedef boost::numeric::ublas::vector<real_scalar_type> real_vector_type;
    typedef mpp::chains::mcmc_chain<real_scalar_type> chain_type;
    typedef std::mt19937 rng_type;
    typedef std::uniform_real_distribution<real_scalar_type> uni_real_dist_type;
    typedef std::uniform_int_distribution<size_t> uni_int_dist_type;
    typedef mpp::hamiltonian::multivariate_normal<
        real_scalar_type> kinetic_energy_type;

    typedef typename std::function<
        real_scalar_type (real_vector_type const &) > log_post_func_type;

    typedef typename std::function<
        real_vector_type (real_vector_type const &)> grad_log_post_func_type;

    static_assert(
        std::is_floating_point<real_scalar_type>::value,
        "The real scalar is expected to be a floating point type."
    );

    hmc_sampler (
        log_post_func_type & log_posterior,
        grad_log_post_func_type & grad_log_posterior,
        size_t const num_dims,
        size_t const max_num_steps,
        real_scalar_type const max_eps,
        real_vector_type const & inv_mass_mat
    ) throw()
    : m_log_posterior(log_posterior)
    , m_grad_log_posterior(grad_log_posterior)
    , m_num_dims(num_dims)
    , m_max_num_steps(max_num_steps)
    , m_max_eps(max_eps)
    , m_inv_mass_mat(inv_mass_mat)
    , m_beta(1)
    ,m_acc_rate(0) {

        if( m_num_dims == size_t(0) ) {
            std::stringstream msg;
            msg << "The number of dimensions = "
                << m_num_dims
                << " should be greater than zero.";
            throw std::length_error(msg.str());
        }

        BOOST_ASSERT_MSG(
            m_num_dims <= size_t(MPP_MAXIMUM_NUMBER_OF_DIMENSIONS),
            "num_dims too big. Please modify the config.hpp and recompile."
        );

        if( m_max_num_steps == size_t(0) ) {
            std::stringstream msg;
            msg << "Maximum number of steps = "
                << m_max_num_steps
                << " in the discretisation of Hamiltonian should be"
                << " greater than zero.";
            throw std::length_error(msg.str());
        }

        BOOST_ASSERT_MSG(
            m_max_num_steps <= size_t(MPP_CLASSIC_HAMILTONIAN_MAX_NUM_STEPS),
            "max_num_steps too big. Please modify the config.hpp and recompile."
        );

        if( m_max_eps <= real_scalar_type(0)
            or m_max_eps > real_scalar_type(1)) {
            std::stringstream msg;
            msg << "Maximum value of epsilon = "
                << m_max_eps
                << " in the discretisation of Hamiltonian should be"
                << " in the intervale (0,1].";
            throw std::domain_error(msg.str());
        }

    }

    chain_type run_sampler(size_t const num_samples,
        real_vector_type const & start_point) throw() {
        BOOST_ASSERT_MSG(
            num_samples <=
                size_t(MPP_MAXIMUM_NUMBER_OF_SAMPLES_PER_RUN_SAMPLER_CALL),
            "num_samples too big. Please modify the config and recompile."
        );

        chain_type hmc_chain(num_samples,m_num_dims);
        uni_real_dist_type uni_real_dist;
        uni_int_dist_type uni_int_dist( size_t(1),m_max_num_steps+size_t(1) );
        kinetic_energy_type kin_eng(m_inv_mass_mat);
        real_vector_type q_1(start_point);
        size_t num_accepted(0);
        size_t num_rejected(0);
        while( num_accepted < num_samples ) {
            real_vector_type q_0(q_1);
            real_scalar_type const eps = m_max_eps*uni_real_dist(m_rng);
            size_t const num_steps = uni_int_dist(m_rng);
            real_vector_type p_0 = kin_eng.generate_sample(m_rng);

            real_scalar_type const h_0 = -m_log_posterior(q_0)
                -kin_eng.log_posterior(p_0);
            leap_frog(m_grad_log_posterior,kin_eng,q_0,p_0,eps,num_steps);
            real_scalar_type const log_post_val = m_log_posterior(q_0);
            real_scalar_type const h_1 = -log_post_val
                -kin_eng.log_posterior(p_0);
            real_scalar_type const delta_h = h_1 - h_0;

            if( not std::isfinite(delta_h) ) {
                std::stringstream msg;
                msg << "delta(H) value is not finite";
                throw std::out_of_range(msg.str());
            }
            real_scalar_type const uni_rand = uni_real_dist(m_rng);
            if(std::log(uni_rand) < -delta_h*m_beta) {
                q_1 = q_0;
                hmc_chain.set_sample(num_accepted,q_1,log_post_val);
                ++num_accepted;
            }
            else {
                ++num_rejected;
            }
        }

        m_acc_rate = real_scalar_type(num_accepted)
            / real_scalar_type(num_accepted + num_rejected);

        return hmc_chain;
    }

    inline real_scalar_type acc_rate() const {
        return m_acc_rate;
    }

private:

    static void leap_frog(
        grad_log_post_func_type & grad_log_posterior,
        kinetic_energy_type & kin_eng,
        real_vector_type & q,
        real_vector_type & p,
        real_scalar_type const eps,
        size_t const num_steps
    ) {
        BOOST_ASSERT_MSG(
            num_steps <= size_t(MPP_CLASSIC_HAMILTONIAN_MAX_NUM_STEPS),
            "m_max_num_steps too big. Please modify the config and recompile."
        );

        BOOST_ASSERT_MSG(
            eps > real_scalar_type(0) and eps <= real_scalar_type(1),
            "epsilon value should be a real number in [0,1]"
        );

        BOOST_ASSERT_MSG(
            p.size() == q.size(),
            "p and q should have identical dimensions"
        );

        real_vector_type dq = grad_log_posterior(q);
        real_vector_type dp = kin_eng.grad_log_posterior(p);

        p += 0.5*eps*dq;
        for(size_t j=0;j<num_steps;++j) {
            dp = kin_eng.grad_log_posterior(p);
            q -= eps*dp;
            dq = grad_log_posterior(q);
            p += eps*dq;
        }
        p -= 0.5*eps*dq;
    }

    log_post_func_type m_log_posterior;
    grad_log_post_func_type m_grad_log_posterior;
    size_t m_num_dims;
    size_t m_max_num_steps;
    real_scalar_type m_max_eps;
    real_vector_type m_inv_mass_mat;
    rng_type m_rng;
    real_scalar_type m_beta;
    real_scalar_type m_acc_rate;
};

}}

#endif // MPP_HAMILTONIAN_CLASSIC_HAMILTONIAN_HPP


#ifndef _INCLUDED_GEOMETRY_HPP_
#define _INCLUDED_GEOMETRY_HPP_
/**
 * @file   geometry.hpp
 * @author Aron Ahmadia <aron@casiphia.local>
 * @date   Tue Feb 26 14:43:29 2008
 *
 * @brief  Geometry - A set of static methods to assist in hypersphere decomposition
 *
 *
 */

#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/matrix.hpp>
//#include "boost_ext/ublas_vector.hpp"
//#include "boost_ext/ublas_matrix.hpp"
#include <boost/version.hpp>

namespace smo {

  const double pi = M_PI;

  typedef double PolarAngle;
  typedef double Area;
  typedef int DimCount;
  typedef int PointCount;
  typedef int RegionCount;

  typedef boost::numeric::ublas::matrix<PolarAngle> Region;
  typedef boost::numeric::ublas::matrix<PolarAngle> SpherePoints;
  typedef boost::numeric::ublas::vector<RegionCount> RegionCounts;
  typedef boost::numeric::ublas::vector<double> CartPoint;
  typedef boost::numeric::ublas::matrix<double> CartPoints;

  typedef struct {
    DimCount d;
    Area capAreaZero;
  } AreaOfCapP;

  class Geometry
  {
  public:
    /**
     * Find the angle that encloses the area of a d-dimensional spherical cap embedded in R^{d+1}
     *
     * @param d dimension of the sphere
     * @param capArea area enclosed by the angle
     *
     * @return angle that ecloses capArea
     */
    static PolarAngle angleOfCap(DimCount d, Area capArea);
    static PolarAngle polarCap(DimCount d, PointCount n);
    static Area regionArea(DimCount d, PointCount n);
    static Area regionArea(DimCount d, Area polarCapArea, PolarAngle phi1, PolarAngle phi2, PointCount n);
    static Area areaOfCap(DimCount d, PolarAngle capPhi);
    static Area areaOfCap(PolarAngle capPhi,AreaOfCapP &p);
    static Area areaOfSphere(DimCount d);
    static Area areaOfCollar(DimCount d, PolarAngle phi1, PolarAngle phi2);
    static void bottomCapRegion(DimCount d, PolarAngle capPhi, Region &r);
    static void topCapRegion(DimCount d, PolarAngle capPhi, Region &r);
    static void sphereRegion(DimCount d, Region &r);
    static PolarAngle circleOffset(PointCount p1, PointCount p2);
    static void toPolar(CartPoints p, SpherePoints &s);
    static void toCart(SpherePoints s, CartPoints &p);
  };
}

#endif


#include "gradient.hpp"
#include "gradient_psv.hpp"
#include "gradient_sh.hpp"

#include <Eigen/Dense>
#include <exception>
#include <fmt/format.h>
#include <memory>

using namespace Eigen;
using grad_psv::GradientPSV;
using grad_sh::GradientSH;

Gradient::Gradient(const Ref<const ArrayXXd> &model)
    : model_(model), nl_(model.rows()) {}

Gradient::~Gradient() = default;

GradientEval::GradientEval(const Ref<const ArrayXXd> &model,
                           const std::string &type) {
  if (type == "rayleigh") {
    grad_ = std::make_unique<GradientPSV>(model);
  } else if (type == "love") {
    grad_ = std::make_unique<GradientSH>(model);
  } else {
    std::string msg = fmt::format("type {:s} is invalid.", type);
    throw std::runtime_error(msg);
  }
}

ArrayXd GradientEval::compute(const double freq, const double c) {
  ArrayXd kvs = grad_->compute(freq, c);
  return kvs;
}


#include "NFmiLambertConformalConicArea.h"
#include "NFmiStringTools.h"
#include <boost/functional/hash.hpp>
#include <fmt/format.h>
#include <macgyver/Exception.h>
#include <cmath>

using namespace std;

// For some reason Boost.Constants does not define quart_pi so we'll just use our own definitions
namespace
{
const double pi = 3.141592653589793238462643383279502884197169399375105820974944592307816406286;
const double half_pi = pi / 2;
const double quart_pi = pi / 4;
}  // namespace

// ----------------------------------------------------------------------
/*!
 * Constructor
 */
// ----------------------------------------------------------------------

NFmiLambertConformalConicArea::NFmiLambertConformalConicArea(const NFmiPoint &theBottomLeftLatLon,
                                                             const NFmiPoint &theTopRightLatLon,
                                                             double theCentralLongitude,
                                                             double theCentralLatitude,
                                                             double theTrueLatitude1,
                                                             double theTrueLatitude2,
                                                             double theRadius,
                                                             bool usePacificView,
                                                             const NFmiPoint &theTopLeftXY,
                                                             const NFmiPoint &theBottomRightXY)
    : NFmiArea(theTopLeftXY, theBottomRightXY, usePacificView),
      itsBottomLeftLatLon(theBottomLeftLatLon),
      itsTopRightLatLon(theTopRightLatLon),
      itsCentralLongitude(theCentralLongitude),
      itsCentralLatitude(theCentralLatitude),
      itsTrueLatitude1(theTrueLatitude1),
      itsTrueLatitude2(theTrueLatitude2),
      itsRadius(theRadius)
{
  try
  {
    Init();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param fKeepWorldRect Undocumented
 */
// ----------------------------------------------------------------------

void NFmiLambertConformalConicArea::Init(bool fKeepWorldRect)
{
  try
  {
    const double lat1 = FmiRad(itsTrueLatitude1);
    const double lat2 = FmiRad(itsTrueLatitude2);
    const double lat0 = FmiRad(itsCentralLatitude);

    if (itsTrueLatitude1 != itsTrueLatitude2)
      itsN = log(cos(lat1) / cos(lat2)) / log(tan(quart_pi + lat2 / 2) / tan(quart_pi + lat1 / 2));
    else
      itsN = sin(lat1);

    itsF = cos(lat1) * pow(tan(quart_pi + lat1 / 2), itsN) / itsN;
    itsRho0 = itsRadius * itsF / pow(tan(quart_pi + lat0 / 2), itsN);

    if (!fKeepWorldRect)
      itsWorldRect =
          NFmiRect(LatLonToWorldXY(itsBottomLeftLatLon), LatLonToWorldXY(itsTopRightLatLon));

    itsXScaleFactor = Width() / itsWorldRect.Width();
    itsYScaleFactor = Height() / itsWorldRect.Height();

    const char *fmt =
        "+proj=lcc +lat_1={} +lat_2={} +lat_0={} +lon_0={} +x_0=0 +y_0=0 +R={} +units=m +wktext "
        "+no_defs +type=crs";
    itsProjStr = fmt::format(fmt,
                             itsTrueLatitude1,
                             itsTrueLatitude2,
                             itsCentralLatitude,
                             itsCentralLongitude,
                             itsRadius);
    itsSpatialReference = std::make_shared<Fmi::SpatialReference>(itsProjStr);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \param theBottomLeftLatLon Undocumented
 * \param theTopRightLatLon Undocumented
 * \return Undocumented
 * \todo Should return an boost::shared_ptr instead
 */
// ----------------------------------------------------------------------

NFmiArea *NFmiLambertConformalConicArea::NewArea(const NFmiPoint &theBottomLeftLatLon,
                                                 const NFmiPoint &theTopRightLatLon,
                                                 bool allowPacificFix) const
{
  try
  {
    if (allowPacificFix)
    {
      PacificPointFixerData fixedPointData =
          NFmiArea::PacificPointFixer(theBottomLeftLatLon, theTopRightLatLon);
      return new NFmiLambertConformalConicArea(fixedPointData.itsBottomLeftLatlon,
                                               fixedPointData.itsTopRightLatlon,
                                               itsCentralLongitude,
                                               itsCentralLatitude,
                                               itsTrueLatitude1,
                                               itsTrueLatitude2,
                                               itsRadius,
                                               fixedPointData.fIsPacific,
                                               TopLeft(),
                                               BottomRight());
    }

    return new NFmiLambertConformalConicArea(theBottomLeftLatLon,
                                             theTopRightLatLon,
                                             itsCentralLongitude,
                                             itsCentralLatitude,
                                             itsTrueLatitude1,
                                             itsTrueLatitude2,
                                             itsRadius,
                                             PacificView(),
                                             TopLeft(),
                                             BottomRight());
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \return Undocumented
 * \todo Should return an boost::shared_ptr instead
 */
// ----------------------------------------------------------------------

NFmiArea *NFmiLambertConformalConicArea::Clone() const
{
  try
  {
    return new NFmiLambertConformalConicArea(*this);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Equality comparison
 *
 * \param theArea The other object being compared to
 * \return True, if the objects are equal
 */
// ----------------------------------------------------------------------

bool NFmiLambertConformalConicArea::operator==(const NFmiLambertConformalConicArea &theArea) const
{
  try
  {
    if ((itsBottomLeftLatLon == theArea.itsBottomLeftLatLon) &&
        (itsTopRightLatLon == theArea.itsTopRightLatLon) &&
        (itsCentralLongitude == theArea.itsCentralLongitude) &&
        (itsCentralLatitude == theArea.itsCentralLatitude) &&
        (itsTrueLatitude1 == theArea.itsTrueLatitude1) &&
        (itsTrueLatitude2 == theArea.itsTrueLatitude2) && (itsRadius == theArea.itsRadius) &&
        (itsWorldRect == theArea.itsWorldRect))
      return true;

    return false;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Inequality comparison
 *
 * \param theArea The other object being compared to
 * \return True, if the objects are not equal
 */
// ----------------------------------------------------------------------

bool NFmiLambertConformalConicArea::operator!=(const NFmiLambertConformalConicArea &theArea) const
{
  try
  {
    return !(*this == theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Equality comparison
 *
 * \param theArea The other object being compared to
 * \return True, if the objects are equal
 */
// ----------------------------------------------------------------------

bool NFmiLambertConformalConicArea::operator==(const NFmiArea &theArea) const
{
  try
  {
    return *this == static_cast<const NFmiLambertConformalConicArea &>(theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Inequality comparison
 *
 * \param theArea The other object being compared to
 * \return True, if the objects are not equal
 */
// ----------------------------------------------------------------------

bool NFmiLambertConformalConicArea::operator!=(const NFmiArea &theArea) const
{
  try
  {
    return !(*this == theArea);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ----------------------------------------------------------------------
/*!
 * Write the object to the given output stream
 *
 * \param file The output stream to write to
 * \return The output stream written to
 */
// ----------------------------------------------------------------------

std::ostream &NFmiLambertConformalConicArea::Write(std::ostream &file) const
{
  try
  {
    NFmiArea::Write(file);

    file << itsBottomLeftLatLon << itsTopRightLatLon << endl
         << itsCentralLongitude << ' ' << itsCentralLatitude << endl
         << itsTrueLatitude1 << ' ' << itsTrueLatitude2 << endl
         << itsRadius << endl;

    int oldPrec = file.precision();
    file.precision(15);
    file << itsWorldRect << endl;

    file.precision(oldPrec);

    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * Read new object contents from the given input stream
 *
 * \param file The input stream to read from
 * \return The input stream read from
 */
// ----------------------------------------------------------------------

std::istream &NFmiLambertConformalConicArea::Read(std::istream &file)
{
  try
  {
    NFmiArea::Read(file);

    file >> itsBottomLeftLatLon >> itsTopRightLatLon;
    PacificView(NFmiArea::IsPacificView(itsBottomLeftLatLon, itsTopRightLatLon));
    file >> itsCentralLongitude >> itsCentralLatitude >> itsTrueLatitude1 >> itsTrueLatitude2 >>
        itsRadius >> itsWorldRect;
    Init();

    return file;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

NFmiArea *NFmiLambertConformalConicArea::CreateNewArea(const NFmiRect &theRect) const
{
  try
  {
    NFmiPoint bottomLeft(ToLatLon(theRect.BottomLeft()));
    NFmiPoint topRight(ToLatLon(theRect.TopRight()));
    NFmiArea *area = new NFmiLambertConformalConicArea(bottomLeft,
                                                       topRight,
                                                       itsCentralLongitude,
                                                       itsCentralLatitude,
                                                       itsTrueLatitude1,
                                                       itsTrueLatitude2,
                                                       itsRadius,
                                                       false,
                                                       TopLeft(),
                                                       BottomRight());
    return area;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const std::string NFmiLambertConformalConicArea::AreaStr() const
{
  try
  {
    std::ostringstream out;
    out << "lcc," << itsCentralLongitude << ',' << itsCentralLatitude << ',' << itsTrueLatitude1;
    if (itsTrueLatitude2 != itsTrueLatitude2 || itsRadius != kRearth)
      out << ',' << itsTrueLatitude2 << ',' << itsRadius;
    out << ':' << BottomLeftLatLon().X() << ',' << BottomLeftLatLon().Y() << ','
        << TopRightLatLon().X() << ',' << TopRightLatLon().Y();
    return out.str();
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------

const std::string NFmiLambertConformalConicArea::WKT() const
{
  try
  {
    const char *fmt = R"(PROJCS["FMI_LambertConic",)"
                      R"(GEOGCS["Unknown",)"
                      R"(DATUM["Unknown",SPHEROID["Sphere",{:.0f},0]],)"
                      R"(PRIMEM["Greenwich",0],)"
                      R"(UNIT["Degree",0.0174532925199433]],)"
                      R"(PROJECTION["Lambert_Conformal_Conic_2SP"],)"
                      R"(PARAMETER["latitude_of_origin",{}],)"
                      R"(PARAMETER["central_meridian",{}],)"
                      R"(PARAMETER["standard_parallel_1",{}],)"
                      R"(PARAMETER["standard_parallel_2",{}],)"
                      R"(UNIT["Metre",1.0]])";
    return fmt::format(fmt,
                       itsRadius,
                       itsCentralLatitude,
                       itsCentralLongitude,
                       itsTrueLatitude1,
                       itsTrueLatitude2);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const NFmiPoint NFmiLambertConformalConicArea::LatLonToWorldXY(
    const NFmiPoint &theLatLonPoint) const
{
  try
  {
    const double lat = FmiRad(theLatLonPoint.Y());

    const double theta = itsN * FmiRad(theLatLonPoint.X() - itsCentralLongitude);
    const double rho = itsRadius * itsF * pow(tan(quart_pi + lat / 2), -itsN);

    const double x = rho * sin(theta);
    const double y = itsRho0 - rho * cos(theta);

    return NFmiPoint(x, y);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const NFmiPoint NFmiLambertConformalConicArea::WorldXYToLatLon(const NFmiPoint &theXYPoint) const
{
  try
  {
    double lambda = 0;
    double phi = 0;

    double x = theXYPoint.X();
    double y = itsRho0 - theXYPoint.Y();

    double rho = std::hypot(x, y);

    if (rho == 0)
    {
      // Either pole
      phi = (itsN > 0 ? half_pi : -half_pi);
    }
    else
    {
      if (itsN < 0)
      {
        rho = -rho;
        x = -x;
        y = -y;
      }
      phi = 2 * atan(pow(itsRadius * itsF / rho, 1 / itsN)) - half_pi;
      lambda = atan2(x, y) / itsN + FmiRad(itsCentralLongitude);
    }

    return NFmiPoint(FmiDeg(lambda), FmiDeg(phi));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const NFmiPoint NFmiLambertConformalConicArea::XYToWorldXY(const NFmiPoint &theXYPoint) const
{
  try
  {
    double xWorld = itsWorldRect.Left() + (theXYPoint.X() - Left()) / itsXScaleFactor;
    double yWorld = itsWorldRect.Bottom() - (theXYPoint.Y() - Top()) / itsYScaleFactor;

    return NFmiPoint(xWorld, yWorld);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const NFmiPoint NFmiLambertConformalConicArea::WorldXYToXY(const NFmiPoint &theWorldXYPoint) const
{
  try
  {
    double x = itsXScaleFactor * (theWorldXYPoint.X() - itsWorldRect.Left()) + Left();
    double y = Top() - itsYScaleFactor * (theWorldXYPoint.Y() - itsWorldRect.Bottom());
    return NFmiPoint(x, y);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const NFmiPoint NFmiLambertConformalConicArea::ToLatLon(const NFmiPoint &theXYPoint) const
{
  try
  {
    // Transform local xy-coordinates into world xy-coordinates (meters).

    double xWorld = itsWorldRect.Left() + (theXYPoint.X() - Left()) / itsXScaleFactor;
    double yWorld = itsWorldRect.Bottom() - (theXYPoint.Y() - Top()) / itsYScaleFactor;

    // Transform world xy-coordinates into geodetic coordinates.

    return WorldXYToLatLon(NFmiPoint(xWorld, yWorld));
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

const NFmiPoint NFmiLambertConformalConicArea::ToXY(const NFmiPoint &theLatLonPoint) const
{
  try
  {
    double xLocal, yLocal;

    // Transform input geodetic coordinates into world coordinates (meters) on xy-plane.
    NFmiPoint latlon(FixLongitude(theLatLonPoint.X()), theLatLonPoint.Y());
    NFmiPoint xyWorld(LatLonToWorldXY(latlon));

    if (xyWorld == NFmiPoint::gMissingLatlon)
    {
      return xyWorld;
    }

    // Finally, transform world xy-coordinates into local xy-coordinates
    xLocal = Left() + itsXScaleFactor * (xyWorld.X() - itsWorldRect.Left());
    yLocal = Top() + itsYScaleFactor * (itsWorldRect.Bottom() - xyWorld.Y());

    return NFmiPoint(xLocal, yLocal);
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}

// ----------------------------------------------------------------------
/*!
 * \brief Hash value
 */
// ----------------------------------------------------------------------

std::size_t NFmiLambertConformalConicArea::HashValue() const
{
  try
  {
    std::size_t hash = NFmiArea::HashValue();
    boost::hash_combine(hash, itsBottomLeftLatLon.HashValue());
    boost::hash_combine(hash, itsTopRightLatLon.HashValue());
    boost::hash_combine(hash, boost::hash_value(itsCentralLongitude));
    boost::hash_combine(hash, boost::hash_value(itsCentralLatitude));
    boost::hash_combine(hash, boost::hash_value(itsTrueLatitude1));
    boost::hash_combine(hash, boost::hash_value(itsTrueLatitude2));
    boost::hash_combine(hash, boost::hash_value(itsRadius));
    return hash;
  }
  catch (...)
  {
    throw Fmi::Exception::Trace(BCP, "Operation failed!");
  }
}
// ======================================================================


/// \ref https://www.hackerrank.com/challenges/quicksort1/problem?h_r=next-challenge&h_v=zen

#include "Algorithms/HackerRank/QuickSort.h"

#include <boost/test/unit_test.hpp>
#include <vector>

using Algorithms::HackerRank::Sorting::QuickSort::quick_sort_partition;
using std::vector;

BOOST_AUTO_TEST_SUITE(Algorithms)
BOOST_AUTO_TEST_SUITE(HackerRank)
BOOST_AUTO_TEST_SUITE(Sorting)
BOOST_AUTO_TEST_SUITE(QuickSort_tests)

//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
BOOST_AUTO_TEST_CASE(QuickSortPartitionPartitions)
{
	vector<int> arr {4, 5, 3, 7, 2};

	const vector<int> partition_result {quick_sort_partition(arr)};

	BOOST_TEST(partition_result[2] == 4);
	BOOST_TEST((partition_result[0] == 2 && partition_result[1] == 3 ||
		partition_result[0] == 3 && partition_result[1] == 2));
	BOOST_TEST((partition_result[3] == 5 && partition_result[4] == 7 ||
		partition_result[3] == 7 && partition_result[4] == 5));
}

BOOST_AUTO_TEST_SUITE_END() // QuickSort_tests
BOOST_AUTO_TEST_SUITE_END() // Sorting
BOOST_AUTO_TEST_SUITE_END() // HackerRank
BOOST_AUTO_TEST_SUITE_END() // Algorithms


/*
 * Copyright 2020-2021 INRIA
 */

#ifndef __eigenpy_decomposition_ldlt_hpp__
#define __eigenpy_decomposition_ldlt_hpp__

#include "eigenpy/eigenpy.hpp"

#include <Eigen/Core>
#include <Eigen/Cholesky>

#include "eigenpy/utils/scalar-name.hpp"

namespace eigenpy
{

  template<typename _MatrixType>
  struct LDLTSolverVisitor
  : public boost::python::def_visitor< LDLTSolverVisitor<_MatrixType> >
  {

    typedef _MatrixType MatrixType;
    typedef typename MatrixType::Scalar Scalar;
    typedef typename MatrixType::RealScalar RealScalar;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,1,MatrixType::Options> VectorXs;
    typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,MatrixType::Options> MatrixXs;
    typedef Eigen::LDLT<MatrixType> Solver;

    template<class PyClass>
    void visit(PyClass& cl) const
    {
      namespace bp = boost::python;
      cl
      .def(bp::init<>("Default constructor"))
      .def(bp::init<Eigen::DenseIndex>(bp::arg("size"),
                                       "Default constructor with memory preallocation"))
      .def(bp::init<MatrixType>(bp::arg("matrix"),
                                "Constructs a LDLT factorization from a given matrix."))

      .def("isNegative",&Solver::isNegative,bp::arg("self"),
           "Returns true if the matrix is negative (semidefinite).")
      .def("isPositive",&Solver::isPositive,bp::arg("self"),
           "Returns true if the matrix is positive (semidefinite).")

      .def("matrixL",&matrixL,bp::arg("self"),
           "Returns the lower triangular matrix L.")
      .def("matrixU",&matrixU,bp::arg("self"),
           "Returns the upper triangular matrix U.")
      .def("vectorD",&vectorD,bp::arg("self"),
           "Returns the coefficients of the diagonal matrix D.")
      .def("transpositionsP",&transpositionsP,bp::arg("self"),
           "Returns the permutation matrix P.")

      .def("matrixLDLT",&Solver::matrixLDLT,bp::arg("self"),
           "Returns the LDLT decomposition matrix.",
           bp::return_internal_reference<>())

      .def("rankUpdate",(Solver & (Solver::*)(const Eigen::MatrixBase<VectorXs> &, const RealScalar &))&Solver::template rankUpdate<VectorXs>,
           bp::args("self","vector","sigma"),
           bp::return_self<>())

#if EIGEN_VERSION_AT_LEAST(3,3,0)
      .def("adjoint",&Solver::adjoint,bp::arg("self"),
           "Returns the adjoint, that is, a reference to the decomposition itself as if the underlying matrix is self-adjoint.",
           bp::return_self<>())
#endif

      .def("compute",(Solver & (Solver::*)(const Eigen::EigenBase<MatrixType> & matrix))&Solver::compute,
           bp::args("self","matrix"),
           "Computes the LDLT of given matrix.",
           bp::return_self<>())

      .def("info",&Solver::info,bp::arg("self"),
           "NumericalIssue if the input contains INF or NaN values or overflow occured. Returns Success otherwise.")
#if EIGEN_VERSION_AT_LEAST(3,3,0)
      .def("rcond",&Solver::rcond,bp::arg("self"),
           "Returns an estimate of the reciprocal condition number of the matrix.")
#endif
      .def("reconstructedMatrix",&Solver::reconstructedMatrix,bp::arg("self"),
           "Returns the matrix represented by the decomposition, i.e., it returns the product: L L^*. This function is provided for debug purpose.")
      .def("solve",&solve<VectorXs>,bp::args("self","b"),
           "Returns the solution x of A x = b using the current decomposition of A.")
      .def("solve",&solve<MatrixXs>,bp::args("self","B"),
           "Returns the solution X of A X = B using the current decomposition of A where B is a right hand side matrix.")

      .def("setZero",&Solver::setZero,bp::arg("self"),
           "Clear any existing decomposition.")
      ;
    }

    static void expose()
    {
      static const std::string classname = "LDLT" + scalar_name<Scalar>::shortname();
      expose(classname);
    }

    static void expose(const std::string & name)
    {
      namespace bp = boost::python;
      bp::class_<Solver>(name.c_str(),
                         "Robust Cholesky decomposition of a matrix with pivoting.\n\n"
                         "Perform a robust Cholesky decomposition of a positive semidefinite or negative semidefinite matrix $ A $ such that $ A = P^TLDL^*P $, where P is a permutation matrix, L is lower triangular with a unit diagonal and D is a diagonal matrix.\n\n"
                         "The decomposition uses pivoting to ensure stability, so that L will have zeros in the bottom right rank(A) - n submatrix. Avoiding the square root on D also stabilizes the computation.",
                         bp::no_init)
      .def(LDLTSolverVisitor());
    }

  private:

    static MatrixType matrixL(const Solver & self) { return self.matrixL(); }
    static MatrixType matrixU(const Solver & self) { return self.matrixU(); }
    static VectorXs vectorD(const Solver & self) { return self.vectorD(); }

    static MatrixType transpositionsP(const Solver & self)
    {
      return self.transpositionsP() * MatrixType::Identity(self.matrixL().rows(),
                                                           self.matrixL().rows());
    }

    template<typename MatrixOrVector>
    static MatrixOrVector solve(const Solver & self, const MatrixOrVector & vec)
    {
      return self.solve(vec);
    }
  };

} // namespace eigenpy

#endif // ifndef __eigenpy_decomposition_ldlt_hpp__


#include <cstdlib>
#include <iostream>
#include <fstream>
#include <iterator>
#include <string>
#include <vector>
#include <boost/lexical_cast.hpp>
#include <CGAL/Exact_predicates_exact_constructions_kernel_with_root_of.h>
#include <CGAL/Construct_theta_graph_2.h>
#include <CGAL/gnuplot_output_2.h>

// select the kernel type
typedef CGAL::Exact_predicates_exact_constructions_kernel_with_root_of   Kernel;
typedef Kernel::Point_2                   Point_2;
typedef Kernel::Direction_2               Direction_2;
/* Note: due to a bug in the boost library, using a directed graph
 * will cause a compilation error with g++ and clang++ when using c++11 standard.
 * See https://lists.boost.org/Archives/boost/2016/05/229458.php.
 */
// define the graph type
typedef boost::adjacency_list<boost::listS,
                              boost::vecS,
                              boost::undirectedS,
                              Point_2
                             > Graph;

int main(int argc, char ** argv)
{
  unsigned int k=4; // By default, no. of cones==4
  std::string filename="data/n9.cin"; // file used by default

  if (argc > 1 &&
      (!strcmp(argv[1],"-h") || !strcmp(argv[1],"--help") || !strcmp(argv[1],"-?")))
  {
    std::cout << "Usage: " << argv[0] << " <no. of cones> <input filename> [<direction-x> <direction-y>]" << std::endl;
    return 1;
  }

  if (argc > 1)
  {
    k = atoi(argv[1]);
    if (k<2) {
      std::cout << "The number of cones should be larger than 1!" << std::endl;
      return 1;
    }
  }

  if (argc > 2)
  { filename=std::string(argv[2]); }

  // open the file containing the vertex list
  std::ifstream inf(filename);
  if (!inf) {
    std::cout << "Cannot open file " << filename << "!" << std::endl;
    return 1;
  }

  Direction_2 initial_direction;
  if (argc == 1 || argc == 3)
    initial_direction = Direction_2(1, 0);  // default initial_direction
  else if (argc == 5)
    initial_direction = Direction_2(atof(argv[3]), atof(argv[4]));
  else {
    std::cout << "Usage: " << argv[0] << " <no. of cones> <input filename> [<direction-x> <direction-y>]" << std::endl;
    return 1;
  }

  // iterators for reading the vertex list file
  std::istream_iterator<Point_2> input_begin( inf );
  std::istream_iterator<Point_2> input_end;

  // initialize the functor
  CGAL::Construct_theta_graph_2<Kernel, Graph> theta(k, initial_direction);
  // create an adjacency_list object
  Graph g;
  // construct the theta graph on the vertex list
  theta(input_begin, input_end, g);

  // obtain the number of vertices in the constructed graph
  boost::graph_traits<Graph>::vertices_size_type n = boost::num_vertices(g);
  // generate gnuplot files for plotting this graph
  std::string file_prefix = "t" + boost::lexical_cast<std::string>(k) + "n" + boost::lexical_cast<std::string>(n);
  CGAL::gnuplot_output_2(g, file_prefix);

  return 0;
}


#include "voronoi.h"

#include <boost/foreach.hpp>

using namespace std;

namespace voronoi
{

template<class Tp_>
void
VoronoiDiagram<Tp_>::clip_infinite_edge(const edge_type& edge,
    point_type& p0, point_type& p1)
  const
{
  const cell_type& cell1 = *edge.cell();
  const cell_type& cell2 = *edge.twin()->cell();
  point_type origin, direction;

  point_type s1 = sites[cell1.source_index()];
  point_type s2 = sites[cell2.source_index()];
  bg::assign_point(origin, s1);
  bg::add_point(origin, s2);
  bg::divide_value(origin, 2);
  bg::set<0>(direction, bg::get<1>(s1) - bg::get<1>(s2));
  bg::set<1>(direction, bg::get<0>(s2) - bg::get<0>(s1));

  coordinate_type koef = boundsx
    / (std::max)(fabs(bg::get<0>(direction)), fabs(bg::get<1>(direction)));
  if (edge.vertex0() == NULL) {
    p0 = point_type(
        bg::get<0>(origin) - bg::get<0>(direction) * koef,
        bg::get<1>(origin) - bg::get<1>(direction) * koef);
  } else {
    p0 = point_type(edge.vertex0()->x(), edge.vertex0()->y());
  }
  if (edge.vertex1() == NULL) {
    p1 = point_type(
        bg::get<0>(origin) + bg::get<0>(direction) * koef,
        bg::get<1>(origin) + bg::get<1>(direction) * koef);
  } else {
    p1 = point_type(edge.vertex1()->x(), edge.vertex1()->y());
  }
}

// Function object to filter-out users whose sites are already known.
template <class Value>
struct is_unknown {
  const vector<int> &index_set;
  is_unknown(const vector<int> &s) : index_set(s) {}
  inline bool operator()(Value const& p) const {
    return index_set[p.second] == -1;
  }
};

template <class Value, class Tp_>
struct inside_cell {
  typedef typename VoronoiDiagram<Tp_>::cell_type cell_type;
  const VoronoiDiagram<Tp_> &vd;
  const cell_type &cell;
  inside_cell(const VoronoiDiagram<Tp_> &v, const cell_type& c)
    : vd(v), cell(c) {}
  inline bool operator()(Value const& p) const {
    return vd.is_inside(cell, p.first); // point inside cell
  }
};

template<class Tp_>
void
VoronoiDiagram<Tp_>::map_knn(void)
{
  // Construct an R-tree for NN(1) queries on the sites.
  // We use this to efficiently find the site that is nearest to each user.
  typedef pair<point_type, int> rvalue_t;
  vector<rvalue_t> svals;
  int site_idx = 0;
  for (auto site = sites.begin(); site != sites.end(); ++site, ++site_idx)
    svals.push_back(make_pair(
          point_type(bg::get<0>(*site), bg::get<1>(*site)),
          site_idx));
  typedef typename bgi::rtree<rvalue_t, bgi::quadratic<16> > voronoi_tree;
  voronoi_tree vtree(svals.begin(), svals.end());

  // The nearest-neighbor is of course the voronoi cell.
  for (size_t user_idx = 0u; user_idx < users.size(); user_idx++)
  {
    const point_type &user = users[user_idx];
    u2s[user_idx] = vtree.qbegin(bgi::nearest(user, 1))->second;
  }
}

// This is an efficient method for mapping user points to their nearest site,
// using a voronoi diagram and range-tree.
// This runs in O(m log m + (m + n) log n) on average for m sites and n users:
//   O(m log m) worst-case to build the voronoi diagram;
//   O(n log n) worst-case to build the user range tree;
//   O(m log n) average-case to find user points in each cell (see below).
// Since Voronoi cells are convex and the Voronoi edge set is O(m), we can
// use the winding check for containment of a query point in amortized O(1)
// time per cell.
// Each containment check in the worst case must consider O(n) users.
// We improve this to O(log n) on average by using a range tree to filter each
// user query down to points which lie in the cell's bounding box. Of course
// this only works when the cells and users are distributed randomly.
// In the worst case, all Voronoi cells may share the same bounding box, and
// all O(n) user points will be checked for all O(m) cells anyway. However, the
// containment check is short-circuited for those users who have already been
// assigned a cell yet, so the overhead is small for the extra comparisons.
template<class Tp_>
void
VoronoiDiagram<Tp_>::map_quick(void)
{
  // Construct an R-tree for range queries on the user points.
  // We use this to efficiently find the user points that lie within each
  // voronoi cell using its bounding box.
  typedef pair<point_type, int> rvalue_t;
  vector<rvalue_t> uvals;
  int user_idx = 0;
  for (auto user = users.begin(); user != users.end(); ++user, ++user_idx)
    uvals.push_back(make_pair(
          point_type(bg::get<0>(*user), bg::get<1>(*user)),
          user_idx));
  bgi::rtree<rvalue_t, bgi::quadratic<16> > utree(uvals.begin(), uvals.end());

  for (auto it = vd.cells().begin(); it != vd.cells().end(); ++it)
  {
    const cell_type &cell = *it;
    // Map the users that lie inside our cell to this site.
    rect_type bbox = boundingRect(cell);
    vector<rvalue_t> query_users;
    utree.query(
           bgi::satisfies(is_unknown<rvalue_t>(u2s))
        && bgi::within(bbox)
        && bgi::satisfies(inside_cell<rvalue_t, Tp_>(*this, cell)),
        back_inserter(query_users));
    BOOST_FOREACH(rvalue_t const& v, query_users)
    {
      int user_idx = v.second;
      u2s[user_idx] = cell.source_index();
    }
  }
}

/*
// This is a brute-force (slow) method for mapping user points to their cells
// simply by associating each user to the site to which it is nearest.
// This runs in O(m*n) for m sites and n users.
template<class Tp_>
void
VoronoiDiagram<Tp_>::map_slow(void)
{
  u2s = vector<int>(users.size(), -1);
  for (size_t user_idx = 0u; user_idx < users.size(); ++user_idx)
  {
    const Point &user = users[user_idx];
    double mindist = std::numeric_limits<double>::infinity();
    int closest_site_idx = -1;
    for (size_t site_idx = 0u; site_idx < sites.size(); ++site_idx)
    {
      const Point &site = sites[site_idx];
      double distance = normalize(site - user);
      if (distance < mindist) {
        mindist = distance;
        closest_site_idx = static_cast<int>(site_idx);
      }
    }
    u2s[user_idx] = closest_site_idx;
  }
}
*/

template<class Tp_>
void
VoronoiDiagram<Tp_>::build(SearchMethod sm)
{
  vd.clear();

  // Always construct the voronoi diagram from the sites so we have edges.
  boost::polygon::construct_voronoi(sites.begin(), sites.end(), &vd);

  // u2s maps user index to nearest facility index
  u2s = vector<int>(users.size(), -1);

  // Now map users to their cells through whichever method we want.
  switch (sm) {
    /*
    case SearchMethod::Slow:
      this->map_slow(); break;
      */
    case SearchMethod::Quick:
      this->map_quick(); break;
    case SearchMethod::KNN:
    case SearchMethod::Default:
      this->map_knn(); break;
    default:
      throw runtime_error("VoronoiDiagram::build(): bad SearchMethod!");
  };
}

// Common instantiations.
template class VoronoiDiagram<double>;

}


#include "stdafx.h"
#include "Triangle.h"
#include <stdexcept>
#include <string>
#include <sstream>
#include <boost/format.hpp>
#include <math.h>

using namespace std;

namespace
{
const double & AssureNotNegative(const double & value, const string & argName = string())
{
	if (value >= 0)
	{
		return value;
	}
	throw invalid_argument(argName.empty() ? "Argument" : argName + " can not be negative.");
}
}

#define ASSURE_NOT_NEGATIVE(value) AssureNotNegative((value), #value)

CTriangle::CTriangle(double side1, double side2, double side3)
	: m_side1(ASSURE_NOT_NEGATIVE(side1))
	, m_side2(AssureNotNegative(side2, "Side 2"))
	, m_side3(AssureNotNegative(side3, "Side 3"))
{
	ValidateSideLength(side1, side2, side3, 1, 2, 3);
	ValidateSideLength(side2, side1, side3, 2, 1, 3);
	ValidateSideLength(side3, side1, side2, 3, 1, 2);
	//if (side1 > side2 + side3)
	//{
	//	throw domain_error("Side 1 can not be greater than sum of side 2 and side 3");
	//}

	//if (side2 > side1 + side3)
	//{
	//	throw domain_error("Side 2 can not be greater than sum of side 1 and side 3");
	//}

	//if (side3 > side1 + side2)
	//{
	//	throw domain_error("Side 3 can not be greater than sum of side 1 and side 2");
	//}
}

double CTriangle::GetSide1()const
{
	return m_side1;
}

double CTriangle::GetSide2()const
{
	return m_side2;
}

double CTriangle::GetSide3()const
{
	return m_side3;
}

double CTriangle::GetPerimeter()const
{
	return m_side1 + m_side2 + m_side3;
}

double CTriangle::GetArea()const
{
	double p = GetPerimeter() / 2;
	return sqrt(p * (p - m_side1) * (p - m_side2) * (p - m_side3));
}

void CTriangle::ValidateSideLength(
	double sideToCheck, double side1, double side2,
	int checkedSideIndex, int side1Index, int side2Index)
{
	if (sideToCheck > side1 + side2)
	{
		throw domain_error((boost::format("Side %1% can not be greater than sum of side %2% and side %3%")
				% checkedSideIndex % side1Index % side2Index).str());

		//ostringstream msg;
		//msg << "Side " << checkedSideIndex << " can not be greater than sum of side "
		//	<< side1Index << " and side " << side2Index;
		//throw domain_error(msg.str());

		//throw domain_error(
		//	"Side " + to_string(checkedSideIndex) +
		//	" can not be greater than sum of side " + to_string(side1Index) +
		//	" and side " + to_string(side2Index));
	}
}


/*
 * Copyright 2009-2020 The VOTCA Development Team (http://www.votca.org)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
#define BOOST_TEST_MAIN

#define BOOST_TEST_MODULE newton_rapson

// Third party includes
#include <boost/test/unit_test.hpp>

// Local VOTCA includes
#include "votca/xtp/newton_rapson.h"

using namespace votca::xtp;
using namespace votca;

BOOST_AUTO_TEST_SUITE(newton_rapson)
class Func {
 public:
  std::pair<double, double> operator()(double x) const {
    std::pair<double, double> value;
    value.first = x * x - 612;
    value.second = 2 * x;
    return value;
  }
};

BOOST_AUTO_TEST_CASE(newton_rapson) {

  Func f;
  Index iterations = 50;
  double tolerance = 1e-9;
  NewtonRapson<Func> n(iterations, tolerance);
  double root = n.FindRoot(f, 10.0);
  BOOST_CHECK_EQUAL(n.getInfo(), NewtonRapson<Func>::Errors::success);
  // see https://en.wikipedia.org/wiki/Newton%27s_method
  BOOST_CHECK_CLOSE(root, 24.738633753, 1e-7);
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @file

  @copyright 2016 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)
*/
//==================================================================================================
#ifndef BOOST_SIMD_CONSTANT_INVEXP_1_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_INVEXP_1_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!

    @ingroup group-constant

    Generates constant 1/e.

    @par Semantic:
    The e constant is the real number such that \f$\log(e) = 1\f$.

    @code
    T r = Invexp_1<T>();
    @endcode

    is similar to:

    @code
    r =  T(0.3678794411714423215955237701614608674458111310317678);
    @endcode

**/
  template<typename T> T Invexp_1();

  namespace functional
  {
    /*!
      @ingroup group-callable-constant

      Generates constant 1/e.

      Generate the  constant invexp_1.

      @return The Invexp_1 constant for the proper type
    **/
    Value Invexp_1<Value>();
  }
} }
#endif

#include <boost/simd/constant/definition/invexp_1.hpp>
#include <boost/simd/arch/common/scalar/constant/constant_value.hpp>
#include <boost/simd/arch/common/simd/constant/constant_value.hpp>

#endif


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#include <boost/python.hpp>

#include <ndhist/stats/excess.hpp>

namespace bp = boost::python;

namespace ndhist {
namespace stats {

void register_excess()
{
    bp::def("excess"
      , &py::excess
      , ( bp::arg("hist")
        , bp::arg("axis")=bp::object()
        )
      , "Calculates the excess kurtosis along the given axis of the given    \n"
        "ndhist object. As in statistics, the excess kurtosis is defined as  \n"
        ":math:`Excess[x] = Kurtosis[x] - 3`.                                \n"
        "It is a measure how normaly distributed the distribution is.        \n"
        "This function generates a projection along the given axis and then  \n"
        "calculates the excess kurtosis.                                     \n"
        "If ``None`` is given as axis argument (the default), the excess     \n"
        "kurtosis for all individual axes of the ndhist object is calculated \n"
        "and returned as a tuple. But if the dimensionality of the histogram \n"
        "is 1, a scalar value is returned.                                   \n"
        "                                                                    \n"
        ".. note:: This function is only defined for ndhist objects with POD \n"
        "          type axis values AND POD type weight values.              \n"
    );
}

}// namespace stats
}// namespace ndhist


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschränkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

#ifndef MTL_MATRIX_MAX_ABS_POS_INCLUDE
#define MTL_MATRIX_MAX_ABS_POS_INCLUDE

#include <utility>
#include <cmath>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/concept/magnitude.hpp>
#include <boost/numeric/mtl/utility/range_generator.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace mtl {

namespace mat {
///Returns pair (row, col) from absolut maximal entry of %matrix A
    template <typename Matrix>
    typename mtl::traits::enable_if_matrix<Matrix, std::pair<typename Collection<Matrix>::size_type, typename Collection<Matrix>::size_type> >::type
    inline max_abs_pos(const Matrix& A)
    {
	vampir_trace<3024> tracer;
	namespace traits = mtl::traits;
	using std::abs;
	typedef typename Collection<Matrix>::value_type   value_type;
	typedef typename Collection<Matrix>::size_type    size_type;

	typename RealMagnitude<value_type>::type max(abs(A[0][0]));
	size_type r= 0, c= 0;

	typename traits::row<Matrix>::type             row(A);
	typename traits::col<Matrix>::type             col(A);
	typename traits::const_value<Matrix>::type     value(A);
	typedef typename traits::range_generator<tag::major, Matrix>::type  cursor_type;

	for (cursor_type cursor = begin<tag::major>(A), cend = end<tag::major>(A); cursor != cend; ++cursor) {
	    typedef typename traits::range_generator<tag::nz, cursor_type>::type icursor_type;
	    for (icursor_type icursor = begin<tag::nz>(cursor), icend = end<tag::nz>(cursor); icursor != icend; ++icursor)
		if (abs(value(*icursor)) > max) {
		    max= abs(value(*icursor));
		    r= row(*icursor);
		    c= col(*icursor);
		}
	}

	return std::make_pair(r, c);
    }

} // namespace matrix

namespace vec {
///Returns position from absolut maximal entry of %vector v
    template <typename Vector>
    typename mtl::traits::enable_if_vector<Vector, typename Collection<Vector>::size_type>::type
    inline max_abs_pos(const Vector& v)
    {
	vampir_trace<2011> tracer;
	using std::abs;
	typedef typename Collection<Vector>::size_type    size_type;
	typedef typename Collection<Vector>::value_type   value_type;

	size_type i= 0;
	size_type max_col= size(v);
	value_type max(abs(v[0]));

	for(size_type j= 1; j < max_col; j++)
	    if(abs(v[j]) > max) {
		max = abs(v[j]);
		i= j;
	    }
	return i;
    }

} // namespace vector

} // namespace mtl

#endif // MTL_MATRIX_MAX_ABS_POS_INCLUDE


#ifndef SETTINGS_HPP
#define SETTINGS_HPP

#include <boost/lexical_cast.hpp>
#include <clstatphys/physics/todalattice.hpp>
#include <clstatphys/ensemble/hybrid_monte_carlo.hpp>
#include <clstatphys/ensemble/modeoccupancy_ensemble_periodic_boundary_fftw.hpp>
#include <common/settings_common.hpp>

using Ensembler = ensemble::ModeOccupancyEnsemblePeriodicBoundaryFFTW;
using Hamiltonian = hamiltonian::TodaLattice;
using MonteCarloSamplerOrigin = ensemble::HybridMonteCarlo;

class MonteCarloSampler: public MonteCarloSamplerOrigin{
public:
  template<class Hamiltonian>
  MonteCarloSampler(
            int num_particles, double temperture, double dt_relax,
            double relax_time, int total_accept, Hamiltonian hamiltonian)
    : MonteCarloSamplerOrigin(num_particles, temperture, dt_relax, relax_time, total_accept)
  {hamiltonian_ = hamiltonian;}

  template<class Rand>
  void montecarlo(std::vector<double> & z, Rand & mt){
    int counter = 0;
    MonteCarloSamplerOrigin::montecarlo(z, counter, hamiltonian_, mt);
  }

private:
  Hamiltonian hamiltonian_;
}; //end MonteCarloSampler definition

struct Settings : public SettingsCommon{
  int N_normalmode = 5; //numer of time step
  int k_initial = 0; //start wave vector filled by initialization
  double E_initial = 1.0; // initial energy

  int total_accept = 10;
  double relax_time = 10;
  double dt_relax = 0.1;
  double temperture = 1.0;

  double J = 1.0; //interaction constant;
  double alpha = 1.0; //interaction constant;

  Settings(int argc, char **argv, int & input_counter)
    : SettingsCommon(argc, argv, input_counter)
  {
    set(argc, argv, input_counter);
  }
  Settings() = default;

  inline void set(int argc, char **argv, int & input_counter){
    if (argc > input_counter) E_initial =        boost::lexical_cast<double>(argv[input_counter]);++input_counter;
    if (argc > input_counter) k_initial =        boost::lexical_cast<int>(argv[input_counter]);++input_counter;
    if (argc > input_counter) N_normalmode =     boost::lexical_cast<int>(argv[input_counter]);++input_counter;
    if (Ns < N_normalmode + k_initial){
      std::cerr << "k_initial + N_noramalmode should be lower than Ns" << std::endl;
      std::exit(1);
    }

    if (argc > input_counter) total_accept       = boost::lexical_cast<int>(argv[input_counter]);++input_counter;
    if (argc > input_counter) relax_time         = boost::lexical_cast<double>(argv[input_counter]);++input_counter;
    if (argc > input_counter) dt_relax           = boost::lexical_cast<double>(argv[input_counter]);++input_counter;
    if (argc > input_counter) temperture         = boost::lexical_cast<double>(argv[input_counter]);++input_counter;

    if (argc > input_counter) J =     boost::lexical_cast<double>(argv[input_counter]);++input_counter;
    if (argc > input_counter) alpha = boost::lexical_cast<double>(argv[input_counter]);++input_counter;
  }

  template <class Dataput>
  inline void declare(Dataput & dataput){
    SettingsCommon::declare(dataput);

    dataput <<  "<<System Depend Settings>> " << std::endl
            <<  "  " << Hamiltonian::name() << std::endl
            <<  "  " << Ensembler::name() << std::endl
            <<  "  " << MonteCarloSampler::name() << std::endl

            // declare normalmode ensemble
            << "  Energy initial : E_initial =" << E_initial << std::endl
            << "  Number of non-zero energy normal modes : N_normalmode =" << N_normalmode << std::endl
            << "  Start wave vector filled by initialization : k_initial =" << k_initial << std::endl

            // declare hybrid monte carlo
            << "  Number of step for fianally accept : total_accept = " << total_accept << std::endl
            << "  Relaxtion time for update : relax_time = " << relax_time << std::endl
            << "  Interbal of time development : dt_relax  = " << dt_relax << std::endl
            << "  Temperture : temperture = " << temperture << std::endl

            // declare hamiltonian
            << "  Coupling constant : J = " << J << std::endl
            << "  Coupling constant : alpha = " << alpha << std::endl;
  }

  Hamiltonian hamiltonian(){
    Lattice lattice_t(Ns);

    std::vector<std::vector<int> > pair_table(
                                              num_particles,
                                              std::vector<int>(N_adj)
                                              );
    lattice_t.create_table(pair_table);

    Hamiltonian hamiltonian_t(
                              num_particles,
                              J,
                              alpha,
                              pair_table,
                              N_adj
                              );
    return hamiltonian_t;
  }

  MonteCarloSampler monte_carlo_sampler(){
    MonteCarloSampler monte_carlo_sampler_t(
                                            num_particles,
                                            temperture,
                                            dt_relax,
                                            relax_time,
                                            total_accept,
                                            hamiltonian()
                                            );
    return monte_carlo_sampler_t;
  }

  Ensembler ensembler(){
    Ensembler ensembler_t(
                          num_particles,
                          k_initial,
                          N_normalmode,
                          E_initial
                          );
    return ensembler_t;
  }

};

#endif


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#include <boost/python.hpp>

#include <ndhist/stats/kurtosis.hpp>

namespace bp = boost::python;

namespace ndhist {
namespace stats {

void register_kurtosis()
{
    bp::def("kurtosis"
      , &py::kurtosis
      , ( bp::arg("hist")
        , bp::arg("axis")=bp::object()
        )
      , "Calculates the kurtosis along the given axis of the given           \n"
        "ndhist object. As in statistics, the kurtosis is defined as         \n"
        ":math:`Kurtosis[x] = (E[x^4] - 4 E[x] E[x^3] + 6 E[x]^2 E[x^2] - 3 E[x]^4) / V[x]^2`.\n"
        "This function generates a projection along the given axis and then  \n"
        "calculates the kurtosis.                                            \n"
        "If ``None`` is given as axis argument (the default), the kurtosis   \n"
        "for all individual axes of the ndhist object is calculated and      \n"
        "returned as a tuple. But if the dimensionality of the histogram is  \n"
        "1, a scalar value is returned.                                      \n"
        "                                                                    \n"
        ".. note:: This function is only defined for ndhist objects with POD \n"
        "          type axis values AND POD type weight values.              \n"
    );
}

}// namespace stats
}// namespace ndhist


/*!
  @file glm.hpp
  @author Klaus K. Holst
  @copyright 2018-2020, Klaus Kähler Holst

  @brief Utility functions for Generalized Linear Models

*/

#pragma once

#ifndef ARMA_R
#define MATHLIB_STANDALONE
#include <armadillo>
#endif
#if defined(ARMA_R)
#include <RcppArmadillo.h>
#endif
#include <cmath>
#include <complex>
#include <cfloat>     // precision of double (DBL_MIN)
#include <functional>  // std::bind for using non-static member function as argument to free function
#include <vector>

using cx_dbl  = std::complex<double>;
using cx_func = std::function<arma::cx_mat(arma::cx_vec theta)>;
using matlist = std::vector<arma::mat>;

namespace target {

  arma::mat expit(arma::mat x);
  arma::cx_mat expit(arma::cx_mat x);
  arma::vec softmax(arma::vec u);
  arma::mat softmax(arma::mat lp, bool ref, bool log);

  // template<typename T>
  // arma::Mat<T> expit(const arma::Mat<T> &x) {
  //   return 1.0/(1+exp(-x));
  // }
  arma::mat expit(arma::mat x);
  arma::cx_mat expit(arma::cx_mat x);

  class IID {
  public:
    arma::mat iid;
    arma::mat vcov;
    IID(): iid(arma::zeros(1, 1)), vcov(arma::zeros(1, 1)) {}
    IID(arma::mat score, arma::mat v): iid(score*v), vcov(v) {}
  };

  IID logistic_iid(const arma::vec &y,
		   const arma::vec &p,
		   const arma::mat &x,
		   const arma::vec &w);

  IID linear_iid(const arma::vec &y,
		 const arma::vec &p,
		 const arma::mat &x,
		 const arma::vec &w);

}  // namespace target


/*
 * This is part of the fl library, a C++ Bayesian filtering library
 * (https://github.com/filtering-library)
 *
 * Copyright (c) 2015 Max Planck Society,
 * 				 Autonomous Motion Department,
 * 			     Institute for Intelligent Systems
 *
 * This Source Code Form is subject to the terms of the MIT License (MIT).
 * A copy of the license can be found in the LICENSE file distributed with this
 * source code.
 */

/**
 * \file robust_sensor_function.hpp
 * \date 2014
 * \author Jan Issac (jan.issac@gmail.com)
 */

#include <gtest/gtest.h>

#include <Eigen/Dense>

#include <fl/filter/gaussian/transform/point_set.hpp>
#include <fl/filter/gaussian/transform/unscented_transform.hpp>

TEST(UnscentedTransformTest, weights)
{
    typedef Eigen::Matrix<double, 10, 1> Point;
    typedef fl::PointSet<Point> Distribution;
    fl::UnscentedTransform ut;
    const double dim = 10;

    EXPECT_DOUBLE_EQ(1., ut.weight_mean_0(dim) + 2*dim * ut.weight_mean_i(dim));
}

template<
    template<typename, int> class PointSet,
    typename Point,
    int Dim
>
void test_mean_transform(PointSet<Point, Dim>& point_set, int dim)
{
    fl::UnscentedTransform ut;

    Point a = Point::Ones(dim) * 9;
    fl::Gaussian<Point> gaussian;

    gaussian.dimension(dim);
    gaussian.mean(a);

    //EXPECT_FALSE(a.isApprox(point_set.mean()));
    ut.forward(gaussian, point_set);
    EXPECT_TRUE(fl::are_similar(a, point_set.mean()));
}

// fixed dimension, fixed number of points
TEST(UnscentedTransformTest, mean_recovery_fixed_fixed)
{
    constexpr size_t dim = 10;
    typedef Eigen::Matrix<double, dim, 1> Point;

    fl::PointSet<
        Point,
        fl::UnscentedTransform::number_of_points(dim)
    > point_set;
    test_mean_transform(point_set, dim);
}

// fixed dimension, dynamic number of points
TEST(UnscentedTransformTest, mean_recovery_fixed_dynamic)
{
    constexpr size_t dim = 10;
    typedef Eigen::Matrix<double, dim, 1> Point;

    fl::PointSet<Point> point_set;
    test_mean_transform(point_set, dim);
}

// dynamic dimension, dynamic number of points
TEST(UnscentedTransformTest, mean_recovery_dynamic_dynamic)
{
    constexpr size_t dim = 10;
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Point;
    typedef fl::PointSet<Point> PointSet;

    PointSet point_set(dim);
    test_mean_transform(point_set, dim);

    EXPECT_EQ(point_set.count_points(),
              fl::UnscentedTransform::number_of_points(dim));
}

// dynamic dimension, fixed number of points
TEST(UnscentedTransformTest, mean_recovery_dynamic_fixed_throw)
{
    constexpr size_t dim = 10;
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Point;

    fl::UnscentedTransform ut;
    Point a = Point::Ones(dim) * 9;

    fl::Gaussian<Point> gaussian;
    gaussian.dimension(dim);
    gaussian.mean(a);

    fl::PointSet<Point, 3> point_set;

    EXPECT_THROW(ut.forward(gaussian, point_set),
                 fl::WrongSizeException);
}

// dynamic dimension, fixed number of points
TEST(UnscentedTransformTest, mean_recovery_dynamic_fixed)
{
    constexpr size_t dim = 10;
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Point;
    typedef fl::PointSet<Point, 21> PointSet;

    PointSet point_set(dim);
    test_mean_transform(point_set, dim);

    EXPECT_EQ(point_set.count_points(),
              fl::UnscentedTransform::number_of_points(dim));
}

template<
    template<typename, int> class PointSet,
    typename Point,
    int Dim
>
void test_covariance_transform(PointSet<Point, Dim>& point_set, int dim)
{
    fl::UnscentedTransform ut;

    typename fl::Gaussian<Point>::SecondMoment cov;
    cov.setRandom(dim, dim);
    cov *= cov.transpose();

    Point a = Point::Random(dim);

    fl::Gaussian<Point> gaussian;
    gaussian.dimension(dim);
    gaussian.mean(a);
    gaussian.covariance(cov);

    EXPECT_NO_THROW(ut.forward(gaussian, point_set));

    EXPECT_TRUE(
        fl::are_similar(
            ( point_set.centered_points() *
              point_set.covariance_weights_vector().asDiagonal() *
              point_set.centered_points().transpose() ),
            cov));
}

TEST(UnscentedTransformTest, covariance_recovery_fixed_fixed)
{
    constexpr size_t dim = 10;
    typedef Eigen::Matrix<double, dim, 1> Point;

    fl::PointSet<
        Point,
        fl::UnscentedTransform::number_of_points(dim)
    > point_set;

    test_covariance_transform(point_set, dim);
}

TEST(UnscentedTransformTest, covariance_recovery_fixed_dynamic)
{
    constexpr size_t dim = 10;
    typedef Eigen::Matrix<double, dim, 1> Point;

    fl::PointSet<Point> point_set;
    test_covariance_transform(point_set, dim);
}

TEST(UnscentedTransformTest, covariance_recovery_dynamic_dynamic)
{
    constexpr size_t dim = 10;
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Point;

    fl::PointSet<Point> point_set(dim);
    test_covariance_transform(point_set, dim);
}

TEST(UnscentedTransformTest, covariance_recovery_dynamic_fixed_throw)
{
    constexpr size_t dim = 10;
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Point;

    fl::UnscentedTransform ut;

    typename fl::Gaussian<Point>::SecondMoment cov;
    cov.setRandom(dim, dim);
    cov *= cov.transpose();

    Point a = Point::Random(dim);

    fl::Gaussian<Point> gaussian;
    gaussian.dimension(dim);
    gaussian.mean(a);
    gaussian.covariance(cov);

    fl::PointSet<Point, 20> point_set;

    EXPECT_THROW(ut.forward(gaussian, point_set),
                 fl::WrongSizeException);
}

TEST(UnscentedTransformTest, covariance_recovery_dynamic_fixed)
{
    constexpr size_t dim = 10;
    typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Point;

    fl::PointSet<Point, 21> point_set;
    point_set.dimension(dim);
    test_covariance_transform(point_set, dim);
}


// Copyright (C) 2021 Christian Brommer, Control of Networked Systems, University of Klagenfurt, Austria.
//
// All rights reserved.
//
// This software is licensed under the terms of the BSD-2-Clause-License with
// no commercial use allowed, the full terms of which are made available
// in the LICENSE file. No license in patents is granted.
//
// You can contact the author at <christian.brommer@ieee.org>

#include <gmock/gmock.h>
#include <mars/core_state.h>
#include <mars/general_functions/utils.h>
#include <mars/sensors/pose/pose_sensor_class.h>
#include <mars/type_definitions/core_state_type.h>
#include <Eigen/Dense>

class mars_bind_sensor_data : public testing::Test
{
public:
};

TEST_F(mars_bind_sensor_data, CTOR)
{
  mars::PoseSensorData sensor_data;

  constexpr int core_state_size = mars::CoreStateType::size_error_;
  int sensor_state_size = sensor_data.state_.cov_size_;
  int full_state_size = core_state_size + sensor_state_size;

  // Check for correct dimensions
  EXPECT_EQ(sensor_data.get_full_cov().size(), full_state_size * full_state_size);

  // Check that all entrys are zero when no data was written
  EXPECT_EQ(sensor_data.get_full_cov(), Eigen::MatrixXd::Zero(full_state_size, full_state_size));

  // Check that the cov is returned correctly, and that the core cov is set to zero
  Eigen::MatrixXd full_cov(full_state_size, full_state_size);
  full_cov.setRandom();
  full_cov = mars::Utils::EnforceMatrixSymmetry(full_cov);

  Eigen::MatrixXd expected_result = full_cov;
  expected_result.block(0, 0, core_state_size, core_state_size) =
      Eigen::Matrix<double, core_state_size, core_state_size>::Zero();

  sensor_data.set_cov(full_cov);
  Eigen::MatrixXd full_cov_return = sensor_data.get_full_cov();

  Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
  std::cout << (full_cov_return - expected_result).format(OctaveFmt) << std::endl;

  EXPECT_EQ(expected_result, full_cov_return);
}