#pragma once

#include <vpp/vpp.hh>
#include <list>
#include <iostream>
#include <iterator>
#include <random>

#include <opencv2/highgui.hpp>
#include <opencv2/videoio.hpp>
#include <vpp/vpp.hh>
#include <vpp/utils/opencv_bridge.hh>
#include <Eigen/Core>
#include "lookup_tables.hh"

using namespace vpp;
using namespace std;
using namespace cv;
using namespace Eigen;

#define PI_FLOAT     3.14159265f
#define PIBY2_FLOAT  1.5707963f

int nb_call_fast = 0;
int real_fast = 0;

// |error| < 0.005
inline
float atan2_approximation2( float y, float x )
{
    if ( x == 0.0f )
    {
        if ( y > 0.0f ) return PIBY2_FLOAT;
        if ( y == 0.0f ) return 0.0f;
        return -PIBY2_FLOAT;
    }
    float atan;
    float z = y/x;
    if ( fabs( z ) < 1.0f )
    {
        atan = z/(1.0f + 0.28f*z*z);
        if ( x < 0.0f )
        {
            if ( y < 0.0f ) return atan - PI_FLOAT;
            return atan + PI_FLOAT;
        }
    }
    else
    {
        atan = PIBY2_FLOAT - z/(z*z + 0.28f);
        if ( y < 0.0f ) return atan - PI_FLOAT;
    }
    return atan;
}

inline double FastArcTan(double x)
{
    return M_PI_4*x - x*(fabs(x) - 1)*(0.2447 + 0.0663*fabs(x));
}

inline float FastArcTan(float dx,float dy)
{
    float val = dy/dx;
    float fabsval = fabs(val);
    //nb_call_fast++;
    if(fabsval < 1)
    {
        //cout << "val " << val << endl;
        //real_fast++;
        return M_PI_4*val - val*(fabsval - 1)*(0.2447 + 0.0663*fabsval);
    }
    else
    {
        return atan(val);
    }
}

inline vint4 getLineFromPoint_Fast(float rho,float theta,int T_theta,int nrows,int ncols,float &cosinus,float &sinus)
{
    int x1,x2,y1,y2;
    x1=x2=y1=y2=0;
    //intersection avec l'axe des x
    float t = 0;
    t = ((2*M_PI*(theta))/ (T_theta-1)) - M_PI;
    float r = rho;
    float eps = 0.01;

    cosinus = cos(t);
    sinus = sin(t);

    if(fabs(sinus)>eps && fabs(cosinus)>eps)
    {
        y1=0;
        x1 = r/cosinus;
        //cout << "premier x1 " << x1 << "  y1 " << y1 << endl;
        if(x1<0 || x1>ncols)
        {
            //cout << "val " << x1 << endl;
            x1 = ncols - 1;
            y1 = -(cosinus/sinus)*x1 + r/sinus;
            //cout << "deuxieme x1 " << x1 << "  y1 " << y1 << endl << endl;
        }
        x2=0;
        y2 = r/sinus;
        if(y2<0 || y2 >nrows)
        {
            y2 = nrows - 1;
            x2 = -(sinus/cosinus)*y2 + r/cosinus;
        }
    }
    else if(fabs(sinus) < eps)
    {
        y1 = 0;
        x1 = r/cosinus;
        x2 = x1;
        y2 = nrows - 1;
    }
    else if(fabs(cosinus) <eps)
    {
        x1 = 0;
        y1 = r/sinus;
        y2 = y1;
        x2 = ncols - 1;
    }

    return vint4(x1,y1,x2,y2);
}

inline vint4 getLineFromPoint(float rho,float theta,int T_theta,int nrows,int ncols)
{
    int x1,x2,y1,y2;
    x1=x2=y1=y2=0;
    //intersection avec l'axe des x
    int rhomax = int(sqrt(pow(ncols,2)+pow(nrows,2)));
    float t = 0;
    t = ((2*M_PI*(theta))/ (T_theta-1)) - M_PI;
    float r = rho;

    y1=0;
    float cosinus = cos(t);
    if(fabs(cosinus)>0.01)
    {
        x1=int(r/cosinus);
        if(cosinus<0)
        {
            for(int b = nrows -1 ; b >=0 ; b--)
            {
                x1 = (int)round((rho - b*sin(t))/cos(t));
                if(x1>0)
                {
                    y1 = b;
                    break;
                }
            }
        }
    }

    //intersection avec l'axe des y
    x2=0;
    float sinus = sin(t);
    if(fabs(sinus) < 0.01)
    {
        x2 = x1;
        y2 = nrows-1;
    }
    else
    {
        y2=int(r/sinus);
        if(y2<0)
        {
            for(int a = ncols-1 ; a>=0 ; a--)
            {
                y2 = (int)round((rho - a*cos(t))/sin(t));
                if(y2>0)
                {
                    x2 = a;
                    break;
                }
            }
        }
    }
    //cout << " rho " << r << " theta " << t << " val " << x[1] <<  endl;
    if(fabs(cosinus)<0.01)
    {
        x1 = ncols-1;
        y1 = y2;
        //continue;
    }
    //cout << "x1 " << x1 << " y1 " << y1 << " x2 " << x2 << " y2 " << y2 << endl;
    return vint4(x1,y1,x2,y2);
}

inline vint4 getLineFromPoint(cv::Mat &out,int rho,int theta,int T_theta,int nrows,int ncols,int pos)
{
    int x1,x2,y1,y2;
    x1=x2=y1=y2=0;
    //intersection avec l'axe des x
    float t = ((2*M_PI*(theta))/ (T_theta-1)) - M_PI;
    float r = rho;

    y1=0;
    float cosinus = cos(t);
    if(fabs(cosinus)>0.01)
    {
        x1=int(r/cosinus);

        if(cosinus<0)
        {
            for(int b = nrows -1 ; b >=0 ; b--)
            {
                x1 = (int)round((rho - b*sin(t))/cos(t));
                if(x1>0)
                {
                    y1 = b;
                    break;
                }
            }
        }
    }
    //intersection avec l'axe des y
    x2=0;
    float sinus = sin(t);
    if(fabs(sinus) < 0.01)
    {
        x2 = x1;
        y2 = nrows-1;
    }
    else
    {
        y2=int(r/sinus);
        if(y2<0)
        {
            for(int a = ncols-1 ; a>=0 ; a--)
            {
                y2 = (int)round((rho - a*cos(t))/sin(t));
                if(y2>0)
                {
                    x2 = a;
                    break;
                }
            }
        }
    }
    //cout << " rho " << r << " theta " << t << " val " << x[1] <<  endl;
    if(fabs(cosinus)<0.01)
    {
        x1 = ncols-1;
        y1 = y2;
        //continue;
    }
    return vint4(x1,y1,x2,y2);
}

inline void addGaussianNoise(float stddev,float mean,image2d<uchar> &img)
{
    std::default_random_engine generator;
    std::normal_distribution<double> dist(mean, stddev);

    pixel_wise(img) | [&] (auto &i) {
        float val = i + dist(generator);
        i = uchar(val);
    };
}

inline void changeLuminosity(int valeur,image2d<uchar> &img)
{
    pixel_wise(img) | [&] (auto &i) {
        float val = valeur + i;
        i = uchar(val);
    };
}

inline bool vector_contains(std::list<vint2> liste,vint2 val,float error)
{
    for(auto &l : liste)
    {
        if((l - val).norm() <= error  )
        {
            return true;
        }
    }
    return false;
}

template <typename Derived, typename Derived2 >
Derived conv2d(const MatrixBase<Derived>& I, const MatrixBase<Derived2> &kernel )
{
    Derived O = Derived::Zero(I.rows(),I.cols());

    typedef typename Derived::Scalar Scalar;
    typedef typename Derived2::Scalar Scalar2;

    int col=0,row=0;
    int KSizeX = kernel.rows();
    int KSizeY = kernel.cols();

    int limitRow = I.rows()-KSizeX;
    int limitCol = I.cols()-KSizeY;

    Derived2 block ;
    Scalar normalization = kernel.sum();
    if ( normalization < 1E-6 )
    {
        normalization=1;
    }
    for ( row = KSizeX; row < limitRow; row++ )
    {
        for ( col = KSizeY; col < limitCol; col++ )
        {
            Scalar b=(static_cast<Derived2>( I.block(row,col,KSizeX,KSizeY ) ).cwiseProduct(kernel)).sum();
            O.coeffRef(row,col) = b;
        }
    }

    return O/normalization;
}

float convolution(Matrix<uchar,3,3> blockPixel, Matrix<float,3,3> kernel)
{
    float normalisation = kernel.sum();
    if ( normalisation < 1E-6 )
    {
        normalisation=1;
    }
    float res = 0;
    for(int i = 0 ; i < 3 ; i++)
    {
        for(int j =0 ; j < 3 ; j++)
        {
            res += blockPixel.coeffRef(i,j)*kernel.coeffRef(i,j);
        }
    }
    return res/normalisation;
}

void gaussian_blur5(image2d<uchar> src,image2d<uchar> dest)
{
    /* start producing Gaussian filter kernel*/
    const double PI = M_PI ;
    double sigma =  2;
    const int kernalWidth=5;
    const int kernalHeight=5;

    float kernalArray[kernalWidth][kernalHeight];

    double total=0;

    //calculate each relavant value to neighour pixels and store it in 2d array
    for(int row=0;row<kernalWidth;row++){

        for(int col=0;col<kernalHeight;col++){

            float value=(1/(2*PI*pow(sigma,2)))*exp(-(pow(row-kernalWidth/2,2)+pow(col-kernalHeight/2,2))/(2*pow(sigma,2)));

            kernalArray[row][col]=value;

            total+=value;
        }
    }

    //Scale value in 2d array in to 1
    for(int row=0;row<kernalWidth;row++){
        for(int col=0;col<kernalHeight;col++){

            kernalArray[row][col]=kernalArray[row][col]/total;

        }
    }

    int verticleImageBound = (kernalHeight-1)/2;
    int horizontalImageBound = (kernalWidth-1)/2;

    pixel_wise(dest, relative_access(src), src.domain()) | [&] (auto& g, auto i, vint2 coord) {
        int x = coord[1];
        int y = coord[0];
        if(y>verticleImageBound && y <src.nrows()-verticleImageBound
                && x > horizontalImageBound && x < src.ncols()-horizontalImageBound)
        {
            float value=0.0;
            //multiply pixel value with corresponding gaussian kernal value
            value = i(2,-2)*kernalArray[0][0] + i(2,-1)*kernalArray[0][1] + i(2,0)*kernalArray[0][2] + i(2,1)*kernalArray[0][3] + i(2,2)*kernalArray[0][4]
                    +   i(1,-2)*kernalArray[1][0] + i(1,-1)*kernalArray[1][1] + i(1,0)*kernalArray[1][2] + i(1,1)*kernalArray[1][3] + i(1,2)*kernalArray[1][4]
                    +   i(0,-2)*kernalArray[2][0] + i(0,-1)*kernalArray[2][1] + i(0,0)*kernalArray[2][2] + i(0,1)*kernalArray[2][3] + i(0,2)*kernalArray[2][4]
                    +   i(-1,-2)*kernalArray[3][0] + i(-1,-1)*kernalArray[3][1] + i(-1,0)*kernalArray[3][2] + i(-1,1)*kernalArray[3][3] + i(-1,2)*kernalArray[3][4]
                    +   i(-2,-2)*kernalArray[4][0] + i(-2,-1)*kernalArray[4][1] + i(-2,0)*kernalArray[4][2] + i(-2,1)*kernalArray[4][3] + i(-2,2)*kernalArray[4][4];
            //grayScaleImage.at<uchar>(kRow+row-verticleImageBound,kCol+col-horizontalImageBound)
            //*kernalArray[kRow][kCol];
            //assign new values to central point
            g = cvRound(value);
        }
        else
        {
            g = i(0,0);
        }
    };
}

template <typename type_>
inline
vint2 inverse_cantor(type_ z)
{
    auto w = floor((sqrt(8 * z + 1) - 1)/2);
    auto t = (w*w + w) / 2;
    int y = int(z - t);
    int x = int(w -y);
    return vint2(x,y);
}

template <typename type_>
inline
type_ cantor(type_ k1,type_ k2)
{
    type_ k =  0.5 * (k1 + k2) * ( k1 + k2 + 1 ) + k2;
    return k;
}

inline
float sign_of_number(float number_)
{
    if(number_<0)
        return -1;
    else if(number_>0)
        return 1;
    else
        return 0;
}

inline
float sinus_lut(float angle)
{
    int indice;
    float sign_res = 1;
    float sign_of_angle = sign_of_number(angle);
    while(fabs(angle)>M_PI)
    {
        //cout << " angle " << angle << endl;
        angle = angle - (sign_of_angle)*2*M_PI;
    }
    if(fabs(angle-sign_of_angle*M_PI)<0.0001 || fabs(angle-sign_of_angle*0.01)<0.0001)
        return 0;
    else if(fabs(angle-sign_of_angle*M_PI_2)<0.0001)
        return sign_of_angle;
    if(angle<0)
    {
        sign_res = -1;
        angle = -angle;
    }
    if(angle>M_PI_2)
    {
        angle = M_PI - angle;
    }
    indice = int(512.0 * angle / M_PI_2);
    return sign_res*sin_lut[indice];
}

inline
float cosinus_lut(float angle)
{
    return sinus_lut(M_PI_2 - angle);
}


/**
 * \ file BesselFilter.cpp
 */

#include <iostream>

#include <simdpp/simd.h>

#include <ATK/Core/PipelineGlobalSinkFilter.h>
#include <ATK/Core/SIMD/QuaternionConvertFilter.h>

#include <ATK/EQ/BesselFilter.h>
#include <ATK/EQ/SimpleIIRFilter.h>
#include <ATK/EQ/SIMD/BesselFilter.h>

#include <ATK/Mock/FFTCheckerFilter.h>
#include <ATK/Mock/SimpleSinusGeneratorFilter.h>

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_NO_MAIN
#include <boost/test/unit_test.hpp>

#define PROCESSSIZE (1024*64)

BOOST_AUTO_TEST_CASE(BOOST_PP_CAT(SIMDPP_ARCH_NAMESPACE, IIRFilter_BesselLowPassCoefficients_test))
{
  ATK::SimpleSinusGeneratorFilter<double> generator1;
  generator1.set_output_sampling_rate(PROCESSSIZE);
  generator1.set_amplitude(1);
  generator1.set_frequency(1000);
  ATK::SimpleSinusGeneratorFilter<double> generator2;
  generator2.set_output_sampling_rate(PROCESSSIZE);
  generator2.set_amplitude(1);
  generator2.set_frequency(100);
  ATK::SimpleSinusGeneratorFilter<double> generator3;
  generator3.set_output_sampling_rate(PROCESSSIZE);
  generator3.set_amplitude(1);
  generator3.set_frequency(2000);
  ATK::SimpleSinusGeneratorFilter<double> generator4;
  generator4.set_output_sampling_rate(PROCESSSIZE);
  generator4.set_amplitude(1);
  generator4.set_frequency(200);

  ATK::RealToQuaternionFilter<double, simdpp::float64<4> > forwardConvert;
  forwardConvert.set_output_delay(3);
  forwardConvert.set_input_sampling_rate(PROCESSSIZE);
  forwardConvert.set_output_sampling_rate(PROCESSSIZE);
  forwardConvert.set_input_port(0, &generator1, 0);
  forwardConvert.set_input_port(1, &generator2, 0);
  forwardConvert.set_input_port(2, &generator3, 0);
  forwardConvert.set_input_port(3, &generator4, 0);

  ATK::SimpleIIRFilter<ATK::BesselLowPassCoefficients<simdpp::float64<4> > > filter;
  filter.set_input_sampling_rate(PROCESSSIZE);
  filter.set_output_sampling_rate(PROCESSSIZE);
  filter.set_cut_frequency(100);
  filter.set_order(3);
  filter.set_input_port(0, &forwardConvert, 0);

  ATK::QuaternionToRealFilter<simdpp::float64<4>, double> backwardConvert;
  backwardConvert.set_input_sampling_rate(PROCESSSIZE);
  backwardConvert.set_output_sampling_rate(PROCESSSIZE);
  backwardConvert.set_input_port(0, &filter, 0);

  ATK::FFTCheckerFilter<double> checker1;
  checker1.set_input_sampling_rate(PROCESSSIZE);
  checker1.set_output_sampling_rate(PROCESSSIZE);
  std::vector<std::pair<int, double> > frequency_checks1;
  frequency_checks1.push_back(std::make_pair(100, 0));
  frequency_checks1.push_back(std::make_pair(1000, 0.03150874916131525));
  frequency_checks1.push_back(std::make_pair(10000, 0));
  checker1.set_checks(frequency_checks1);
  checker1.set_input_port(0, &backwardConvert, 0);
  ATK::FFTCheckerFilter<double> checker2;
  checker2.set_input_sampling_rate(PROCESSSIZE);
  checker2.set_output_sampling_rate(PROCESSSIZE);
  std::vector<std::pair<int, double> > frequency_checks2;
  frequency_checks2.push_back(std::make_pair(10, 0));
  frequency_checks2.push_back(std::make_pair(100, 0.6984126292895364));
  frequency_checks2.push_back(std::make_pair(1000, 0));
  checker2.set_checks(frequency_checks2);
  checker2.set_input_port(0, &backwardConvert, 1);
  ATK::FFTCheckerFilter<double> checker3;
  checker3.set_input_sampling_rate(PROCESSSIZE);
  checker3.set_output_sampling_rate(PROCESSSIZE);
  std::vector<std::pair<int, double> > frequency_checks3;
  frequency_checks3.push_back(std::make_pair(100, 0));
  frequency_checks3.push_back(std::make_pair(1000, 0));
  frequency_checks3.push_back(std::make_pair(2000, 0.01112234417159909));
  checker3.set_checks(frequency_checks3);
  checker3.set_input_port(0, &backwardConvert, 2);
  ATK::FFTCheckerFilter<double> checker4;
  checker4.set_input_sampling_rate(PROCESSSIZE);
  checker4.set_output_sampling_rate(PROCESSSIZE);
  std::vector<std::pair<int, double> > frequency_checks4;
  frequency_checks4.push_back(std::make_pair(100, 0));
  frequency_checks4.push_back(std::make_pair(200, 0.3285828399406346));
  frequency_checks4.push_back(std::make_pair(1000, 0));
  checker4.set_checks(frequency_checks4);
  checker4.set_input_port(0, &backwardConvert, 3);

  ATK::PipelineGlobalSinkFilter sink;
  sink.set_input_sampling_rate(PROCESSSIZE);
  sink.set_output_sampling_rate(PROCESSSIZE);
  sink.add_filter(&checker1);
  sink.add_filter(&checker2);
  sink.add_filter(&checker3);
  sink.add_filter(&checker4);

  backwardConvert.process(PROCESSSIZE);

  sink.process(PROCESSSIZE);
}

#ifdef SIMDPP_EMIT_DISPATCHER
BOOST_AUTO_TEST_CASE(IIRFilter_BesselLowPassCoefficients_test_dispatccher)
{
  ATK::SimpleSinusGeneratorFilter<double> generator1;
  generator1.set_output_sampling_rate(PROCESSSIZE);
  generator1.set_amplitude(1);
  generator1.set_frequency(1000);
  ATK::SimpleSinusGeneratorFilter<double> generator2;
  generator2.set_output_sampling_rate(PROCESSSIZE);
  generator2.set_amplitude(1);
  generator2.set_frequency(100);
  ATK::SimpleSinusGeneratorFilter<double> generator3;
  generator3.set_output_sampling_rate(PROCESSSIZE);
  generator3.set_amplitude(1);
  generator3.set_frequency(2000);
  ATK::SimpleSinusGeneratorFilter<double> generator4;
  generator4.set_output_sampling_rate(PROCESSSIZE);
  generator4.set_amplitude(1);
  generator4.set_frequency(200);

  auto forwardConvert = ATK::createRealToQuaternionFilter<double>();
  forwardConvert->set_output_delay(3);
  forwardConvert->set_input_sampling_rate(PROCESSSIZE);
  forwardConvert->set_output_sampling_rate(PROCESSSIZE);
  forwardConvert->set_input_port(0, &generator1, 0);
  forwardConvert->set_input_port(1, &generator2, 0);
  forwardConvert->set_input_port(2, &generator3, 0);
  forwardConvert->set_input_port(3, &generator4, 0);

  auto filter = ATK::createLowPassBesselFilter<double, 4>();
  filter->set_input_sampling_rate(PROCESSSIZE);
  filter->set_output_sampling_rate(PROCESSSIZE);
  dynamic_cast<ATK::SingleCutFrequencyInterface<double>*>(filter.get())->set_cut_frequency(100);
  dynamic_cast<ATK::OrderInterface*>(filter.get())->set_order(3);
  filter->set_input_port(0, forwardConvert.get(), 0);

  auto backwardConvert = ATK::createQuaternionToRealFilter<double>();
  backwardConvert->set_input_sampling_rate(PROCESSSIZE);
  backwardConvert->set_output_sampling_rate(PROCESSSIZE);
  backwardConvert->set_input_port(0, filter.get(), 0);

  ATK::FFTCheckerFilter<double> checker1;
  checker1.set_input_sampling_rate(PROCESSSIZE);
  checker1.set_output_sampling_rate(PROCESSSIZE);
  std::vector<std::pair<int, double> > frequency_checks1;
  frequency_checks1.push_back(std::make_pair(100, 0));
  frequency_checks1.push_back(std::make_pair(1000, 0.03150874916131525));
  frequency_checks1.push_back(std::make_pair(10000, 0));
  checker1.set_checks(frequency_checks1);
  checker1.set_input_port(0, backwardConvert.get(), 0);
  ATK::FFTCheckerFilter<double> checker2;
  checker2.set_input_sampling_rate(PROCESSSIZE);
  checker2.set_output_sampling_rate(PROCESSSIZE);
  std::vector<std::pair<int, double> > frequency_checks2;
  frequency_checks2.push_back(std::make_pair(10, 0));
  frequency_checks2.push_back(std::make_pair(100, 0.6984126292895364));
  frequency_checks2.push_back(std::make_pair(1000, 0));
  checker2.set_checks(frequency_checks2);
  checker2.set_input_port(0, backwardConvert.get(), 1);
  ATK::FFTCheckerFilter<double> checker3;
  checker3.set_input_sampling_rate(PROCESSSIZE);
  checker3.set_output_sampling_rate(PROCESSSIZE);
  std::vector<std::pair<int, double> > frequency_checks3;
  frequency_checks3.push_back(std::make_pair(100, 0));
  frequency_checks3.push_back(std::make_pair(1000, 0));
  frequency_checks3.push_back(std::make_pair(2000, 0.01112234417159909));
  checker3.set_checks(frequency_checks3);
  checker3.set_input_port(0, backwardConvert.get(), 2);
  ATK::FFTCheckerFilter<double> checker4;
  checker4.set_input_sampling_rate(PROCESSSIZE);
  checker4.set_output_sampling_rate(PROCESSSIZE);
  std::vector<std::pair<int, double> > frequency_checks4;
  frequency_checks4.push_back(std::make_pair(100, 0));
  frequency_checks4.push_back(std::make_pair(200, 0.3285828399406346));
  frequency_checks4.push_back(std::make_pair(1000, 0));
  checker4.set_checks(frequency_checks4);
  checker4.set_input_port(0, backwardConvert.get(), 3);

  ATK::PipelineGlobalSinkFilter sink;
  sink.set_input_sampling_rate(PROCESSSIZE);
  sink.set_output_sampling_rate(PROCESSSIZE);
  sink.add_filter(&checker1);
  sink.add_filter(&checker2);
  sink.add_filter(&checker3);
  sink.add_filter(&checker4);

  backwardConvert->process(PROCESSSIZE);

  sink.process(PROCESSSIZE);
}
#endif


#include <cctbx/boost_python/flex_fwd.h>

#include <cctbx/sgtbx/sym_equiv_sites.h>
#include <cctbx/import_scitbx_af.h>
#include <scitbx/boost_python/is_polymorphic_workaround.h>
#include <scitbx/array_family/versa.h>
#include <boost/python/module.hpp>
#include <boost/python/class.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/return_internal_reference.hpp>
#include <vector>

namespace cctbx { namespace boost_python { namespace emma_ext {

  struct add_pair
  {
    double tolerance_;
    af::shared<scitbx::vec3<double> > ref_model2_sites_;
    af::versa<bool, af::c_grid<2> > pair_flags_;
    std::vector<sgtbx::sym_equiv_sites<> > equiv1_;
    sgtbx::rt_mx eucl_symop_;
    int new_pair_1_;
    int new_pair_2_;

    add_pair(double tolerance,
             uctbx::unit_cell const& unit_cell,
             sgtbx::space_group const& space_group,
             double min_distance_sym_equiv,
             af::const_ref<scitbx::vec3<double> > const& ref_model1_sites,
             af::shared<scitbx::vec3<double> > const& ref_model2_sites)
    :
      tolerance_(tolerance),
      ref_model2_sites_(ref_model2_sites),
      pair_flags_(af::versa<bool, af::c_grid<2> >(
        af::c_grid<2>(ref_model1_sites.size(),
                      ref_model2_sites.size())))
    {
      for(std::size_t i=0;i<ref_model1_sites.size();i++) {
        sgtbx::site_symmetry site_symmetry(
          unit_cell, space_group,
          ref_model1_sites[i], min_distance_sym_equiv);
        equiv1_.push_back(sgtbx::sym_equiv_sites<>(site_symmetry));
      }
    }

    sgtbx::sym_equiv_sites<> const&
    equiv1(std::size_t i) const { return equiv1_[i]; }

    void
    next_pivot(af::tiny<bool, 3> const& continuous_shift_flags,
               sgtbx::rt_mx const& eucl_symop,
               scitbx::vec3<double> const& adjusted_shift,
               af::const_ref<int> const& singles1,
               af::const_ref<int> const& singles2)
    {
      CCTBX_ASSERT(ref_model2_sites_.size() == pair_flags_.accessor()[1]);
      pair_flags_.fill(false);
      eucl_symop_ = eucl_symop;
      CCTBX_ASSERT(equiv1_.size() > 0);
      bool no_continuous_shifts = continuous_shift_flags.all_eq(false);
      uctbx::unit_cell const& unit_cell = equiv1_[0].unit_cell();
      af::ref<bool, af::c_grid<2> > pair_flags_ref = pair_flags_.ref();
      for (const int* is2 = singles2.begin(); is2 != singles2.end(); is2++) {
        scitbx::vec3<double>
          c2 = eucl_symop_ * ref_model2_sites_[*is2] + adjusted_shift;
        for (const int* is1 = singles1.begin(); is1 != singles1.end(); is1++) {
          sgtbx::min_sym_equiv_distance_info<> dist_info(equiv1_[*is1], c2);
          double dist = dist_info.dist();
          if (no_continuous_shifts) {
            if (dist < tolerance_) {
              pair_flags_ref(*is1,*is2) = true;
            }
          }
          else if (dist < 4 * tolerance_) {
            // ensure that this pair can be matched within 1 * tolerance.
            // (not entirely sure that this is safe under all circumstances.)
            fractional<> const& diff = dist_info.diff();
            fractional<> diff_allowed;
            for (std::size_t j=0;j<3;j++) {
              diff_allowed[j] = (continuous_shift_flags[j] ? diff[j] : 0);
            }
            double dist_allowed = unit_cell.length(diff_allowed);
            if (dist_allowed < tolerance_) {
              pair_flags_ref(*is1,*is2) = true;
            }
          }
        }
      }
    }

    bool
    next_pair(scitbx::vec3<double> const& adjusted_shift,
              af::const_ref<int> const& singles1,
              af::const_ref<int> const& singles2)
    {
      double shortest_dist = 2 * tolerance_;
      bool have_new_pair = false;
      new_pair_1_ = 0;
      new_pair_2_ = 0;
      af::const_ref<bool, af::c_grid<2> >
        pair_flags_ref = pair_flags_.const_ref();
      for (const int* is2 = singles2.begin(); is2 != singles2.end(); is2++) {
        scitbx::vec3<double>
          c2 = eucl_symop_ * ref_model2_sites_[*is2] + adjusted_shift;
        for (const int* is1 = singles1.begin(); is1 != singles1.end(); is1++) {
          if (pair_flags_ref(*is1,*is2)) {
            sgtbx::min_sym_equiv_distance_info<> dist_info(equiv1_[*is1], c2);
            double dist = dist_info.dist();
            if (dist < shortest_dist) {
              shortest_dist = dist;
              have_new_pair = true;
              new_pair_1_ = *is1;
              new_pair_2_ = *is2;
            }
          }
        }
      }
      return have_new_pair;
    }

    int
    new_pair_1() const { return new_pair_1_; }

    int
    new_pair_2() const { return new_pair_2_; }
  };

}}} // namespace cctbx::boost_python::emma_ext

SCITBX_BOOST_IS_POLYMORPHIC_WORKAROUND(cctbx::boost_python::emma_ext::add_pair)

namespace cctbx { namespace boost_python { namespace emma_ext {

  void init_module()
  {
    using namespace boost::python;

    typedef return_internal_reference<> rir;

    class_<add_pair>("add_pair", no_init)
      .def(init<double,
                uctbx::unit_cell const&,
                sgtbx::space_group const&,
                double,
                af::const_ref<scitbx::vec3<double> > const&,
                af::shared<scitbx::vec3<double> > const&>())
      .def("equiv1", &add_pair::equiv1, rir())
      .def("next_pivot", &add_pair::next_pivot)
      .def("next_pair", &add_pair::next_pair)
      .def("new_pair_1", &add_pair::new_pair_1)
      .def("new_pair_2", &add_pair::new_pair_2)
    ;
  }

}}} // namespace cctbx::boost_python::emma_ext

BOOST_PYTHON_MODULE(cctbx_emma_ext)
{
  cctbx::boost_python::emma_ext::init_module();
}


#define EIGEN_NO_DEBUG
#include "operators.hpp"

#include <boost/range/adaptor/reversed.hpp>
#include <vector>

using Eigen::VectorXd;

namespace space {

bool Operator::FeasibleVector(const Eigen::VectorXd &vec) const {
  for (int i = 0; i < triang_.V; ++i)
    if (!IsDof(i) && vec[i] != 0) return false;

  return true;
}

Eigen::MatrixXd Operator::ToMatrix() const {
  Eigen::MatrixXd A = Eigen::MatrixXd::Zero(triang_.V, triang_.V);
  for (int i = 0; i < triang_.V; ++i) {
    Eigen::VectorXd v = Eigen::VectorXd::Zero(triang_.V);
    if (IsDof(i)) v[i] = 1;
    Apply(v);
    A.col(i) = v;
  }
  return A;
}

template <class ForwardOp>
ForwardOperator<ForwardOp>::ForwardOperator(const TriangulationView &triang,
                                            OperatorOptions opts)
    : Operator(triang, opts) {
  if (opts_.build_mat) InitializeMatrixSingleScale();
}

template <class ForwardOp>
void ForwardOperator<ForwardOp>::Apply(Eigen::VectorXd &v) const {
  assert(FeasibleVector(v));

  // Hierarhical basis to single scale basis.
  ApplyHierarchToSingle(v);
  assert(FeasibleVector(v));

  // Apply the operator in single scale.
  ApplySingleScale(v);
  assert(FeasibleVector(v));

  // Return back to hierarhical basis.
  ApplyTransposeHierarchToSingle(v);
  assert(FeasibleVector(v));
}

template <class ForwardOp>
void ForwardOperator<ForwardOp>::ApplyHierarchToSingle(VectorXd &w) const {
  for (uint vi = triang_.InitialVertices(); vi < triang_.V; ++vi)
    for (auto gp : triang_.Godparents(vi)) w[vi] = w[vi] + 0.5 * w[gp];
}

template <class ForwardOp>
void ForwardOperator<ForwardOp>::ApplyTransposeHierarchToSingle(
    VectorXd &w) const {
  for (uint vi = triang_.V - 1; vi >= triang_.InitialVertices(); --vi)
    for (auto gp : triang_.Godparents(vi))
      if (IsDof(gp)) w[gp] = w[gp] + 0.5 * w[vi];
}

template <class ForwardOp>
void ForwardOperator<ForwardOp>::ApplySingleScale(Eigen::VectorXd &v) const {
  if (opts_.build_mat) {
    v = matrix_ * v;
  } else {
    Eigen::VectorXd result = Eigen::VectorXd::Zero(v.rows());
    for (const auto &[elem, Vids] : triang_.element_leaves()) {
      auto &&element_mat = ForwardOp::ElementMatrix(elem, opts_);

      for (uint i = 0; i < 3; ++i)
        if (IsDof(Vids[i]))
          for (uint j = 0; j < 3; ++j)
            if (IsDof(Vids[j]))
              result[Vids[i]] += v[Vids[j]] * element_mat.coeff(i, j);
    }
    v = result;
  }
}

template <class ForwardOp>
void ForwardOperator<ForwardOp>::InitializeMatrixSingleScale() {
  std::vector<Eigen::Triplet<double>> triplets;
  triplets.reserve(triang_.element_leaves().size() * 3);

  for (const auto &[elem, Vids] : triang_.element_leaves()) {
    auto &&element_mat = ForwardOp::ElementMatrix(elem, opts_);

    for (uint i = 0; i < 3; ++i)
      if (IsDof(Vids[i]))
        for (uint j = 0; j < 3; ++j)
          if (IsDof(Vids[j]))
            triplets.emplace_back(Vids[i], Vids[j], element_mat(i, j));
  }

  matrix_ = Eigen::SparseMatrix<double>(triang_.V, triang_.V);
  matrix_.setFromTriplets(triplets.begin(), triplets.end());
}

inline Eigen::Matrix3d MassOperator::ElementMatrix(
    const Element2D *elem, const OperatorOptions &opts) {
  static Eigen::Matrix3d elem_mat =
      (Eigen::Matrix3d() << 2, 1, 1, 1, 2, 1, 1, 1, 2).finished();
  return elem->area() / 12.0 * elem_mat;
}

inline const Eigen::Matrix3d &StiffnessOperator::ElementMatrix(
    const Element2D *elem, const OperatorOptions &opts) {
  return elem->StiffnessMatrix();
}

inline Eigen::Matrix3d StiffPlusScaledMassOperator::ElementMatrix(
    const Element2D *elem, const OperatorOptions &opts) {
  // alpha * Stiff + 2^|labda| * Mass.
  return opts.alpha * StiffnessOperator::ElementMatrix(elem, opts) +
         (1LL << opts.time_level) * MassOperator::ElementMatrix(elem, opts);
}

BackwardOperator::BackwardOperator(const TriangulationView &triang,
                                   OperatorOptions opts)
    : Operator(triang, opts) {
  std::vector<int> dof_mapping;
  dof_mapping.reserve(triang_.V);
  for (int i = 0; i < triang_.V; i++)
    if (IsDof(i)) dof_mapping.push_back(i);
  if (dof_mapping.size() == 0) return;

  std::vector<Eigen::Triplet<double>> triplets, tripletsT;
  triplets.reserve(dof_mapping.size());
  tripletsT.reserve(dof_mapping.size());
  for (int i = 0; i < dof_mapping.size(); i++) {
    triplets.emplace_back(i, dof_mapping[i], 1.0);
    tripletsT.emplace_back(dof_mapping[i], i, 1.0);
  }
  transform_ = Eigen::SparseMatrix<double>(dof_mapping.size(), triang_.V);
  transform_.setFromTriplets(triplets.begin(), triplets.end());
  transformT_ = Eigen::SparseMatrix<double>(triang_.V, dof_mapping.size());
  transformT_.setFromTriplets(tripletsT.begin(), tripletsT.end());
}

void BackwardOperator::ApplyInverseHierarchToSingle(VectorXd &w) const {
  for (uint vi = triang_.V - 1; vi >= triang_.InitialVertices(); --vi)
    for (auto gp : triang_.Godparents(vi)) w[vi] = w[vi] - 0.5 * w[gp];
}

void BackwardOperator::ApplyTransposeInverseHierarchToSingle(
    VectorXd &w) const {
  for (uint vi = triang_.InitialVertices(); vi < triang_.V; ++vi)
    for (auto gp : triang_.Godparents(vi))
      if (IsDof(gp)) w[gp] = w[gp] - 0.5 * w[vi];
}

void BackwardOperator::Apply(Eigen::VectorXd &v) const {
  assert(FeasibleVector(v));

  // Hierarchical basis to single scale.
  ApplyTransposeInverseHierarchToSingle(v);
  assert(FeasibleVector(v));

  // Apply in single sale.
  ApplySingleScale(v);
  assert(FeasibleVector(v));

  // Single scale to hierarchical basis.
  ApplyInverseHierarchToSingle(v);
  assert(FeasibleVector(v));
}

template <typename ForwardOp>
DirectInverse<ForwardOp>::DirectInverse(const TriangulationView &triang,
                                        OperatorOptions opts)
    : BackwardOperator(triang, opts) {
  if (transform_.cols() > 0) {
    OperatorOptions force_build = opts;
    force_build.build_mat = true;
    auto matrix = ForwardOp(triang, force_build).MatrixSingleScale();
    matrix = transform_ * matrix * transformT_;
    solver_.analyzePattern(matrix);
    solver_.factorize(matrix);
  }
}

template <typename ForwardOp>
void DirectInverse<ForwardOp>::ApplySingleScale(Eigen::VectorXd &vec_SS) const {
  if (transform_.cols() > 0)
    vec_SS = transformT_ * solver_.solve(transform_ * vec_SS);
  else
    vec_SS.setZero();
}

template <typename ForwardOp>
CGInverse<ForwardOp>::CGInverse(const TriangulationView &triang,
                                OperatorOptions opts)
    : BackwardOperator(triang, opts) {
  OperatorOptions force_build = opts;
  force_build.build_mat = true;
  auto matrix = ForwardOp(triang, force_build).MatrixSingleScale();
  solver_.compute(transform_ * matrix * transformT_);
}

template <typename ForwardOp>
void CGInverse<ForwardOp>::ApplySingleScale(Eigen::VectorXd &vec_SS) const {
  if (transform_.cols() > 0)
    vec_SS = transformT_ * solver_.solve(transform_ * vec_SS);
  else
    vec_SS.setZero();
}

// Define the class variables.
template <typename ForwardOp>
std::vector<std::vector<std::pair<uint, double>>>
    MultigridPreconditioner<ForwardOp>::row_mat;
template <typename ForwardOp>
std::vector<std::vector<Element2D *>>
    MultigridPreconditioner<ForwardOp>::patches;
template <typename ForwardOp>
std::vector<std::vector<uint>>
    MultigridPreconditioner<ForwardOp>::vertices_relaxation;

template <typename ForwardOp>
MultigridPreconditioner<ForwardOp>::MultigridPreconditioner(
    const TriangulationView &triang, OperatorOptions opts)
    : BackwardOperator(triang, opts),
      forward_op_(triang, opts),
      // Note that this will leave initial_triang_solver_ with dangling
      // reference, but it doesn't matter for our purpose..
      initial_triang_solver_(triang.InitialTriangulationView(), opts) {}

namespace {
inline std::array<uint, 3> Vids(Element2D *elem) {
  return {*elem->vertices()[0]->template data<uint>(),
          *elem->vertices()[1]->template data<uint>(),
          *elem->vertices()[2]->template data<uint>()};
}
}  // namespace

template <typename ForwardOp>
inline void MultigridPreconditioner<ForwardOp>::RowMatrix(
    uint vertex, std::vector<std::pair<uint, double>> &result) const {
  assert(IsDof(vertex));

  const auto &patch = patches[vertex];
  result.clear();
  result.reserve(patch.size() * 3);
  for (auto elem : patch) {
    const auto elem_Vids = Vids(elem);
    const auto &elem_mat = ForwardOp::ElementMatrix(elem, opts_);
    for (uint i = 0; i < 3; ++i)
      if (elem_Vids[i] == vertex)
        for (uint j = 0; j < 3; ++j)
          if (IsDof(elem_Vids[j]))
            result.emplace_back(elem_Vids[j], elem_mat.coeff(i, j));
  }
  //  std::sort(
  //      result.begin(), result.end(),
  //      [](const std::pair<uint, double> &p1, const std::pair<uint, double>
  //      &p2) {
  //        return p1.first < p2.first;
  //      });
  //  uint j = 0;
  //  for (uint i = 1; i < result.size(); i++) {
  //    if (result[i].first == result[j].first)
  //      result[j].second += result[i].second;
  //    else
  //      result[++j] = result[i];
  //  }
  //  result.resize(j + 1);
}

template <typename ForwardOp>
void MultigridPreconditioner<ForwardOp>::InitializeMultigridMatrix() const {
  const uint V = triang_.V;
  patches.resize(V);
  row_mat.resize(V * 3);

  // Store indices in the tree.
  std::vector<uint> indices(V);
  const std::vector<Vertex *> &vertices = triang_.vertices();
  for (uint i = 0; i < V; ++i) {
    indices[i] = i;
    vertices[i]->set_data(&indices[i]);
    patches[i].clear();
    patches[i].reserve(4);
  }

  // Initialize patches var with the triangulation on the finest level.
  for (const auto &[elem, Vids] : triang_.element_leaves())
    for (int vi : Vids) patches[vi].emplace_back(elem);

  // Keep track of vertices included on the current level.
  std::vector<bool> included(V, false);
  vertices_relaxation.resize(triang_.J + 1);
  uint idx = 0;
  for (int k = triang_.J; k >= 1; --k) {
    vertices_relaxation[k].clear();
    for (uint vertex = triang_.VerticesPerLevel(k);
         vertex < triang_.VerticesPerLevel(k + 1); vertex++) {
      const auto godparents = triang_.Godparents(vertex);
      for (uint vi : {vertex, godparents[0], godparents[1]})
        if (!included[vi] && IsDof(vi)) {
          included[vi] = true;
          vertices_relaxation[k].push_back(vi);
          RowMatrix(vi, row_mat[idx++]);
        }
    }

    // Reset the included variable.
    for (uint vertex : vertices_relaxation[k]) included[vertex] = false;

    // Coarsen patches, from k to k - 1.
    for (uint vertex = triang_.VerticesPerLevel(k);
         vertex < triang_.VerticesPerLevel(k + 1); vertex++) {
      for (const auto &elem : vertices[vertex]->parents_element) {
        assert(elem->level() == k - 1);
        for (auto vi : Vids(elem)) {
          // We must remove the children of elem from the patch at vi.
          auto &patch = patches[vi];
          for (const auto &child : elem->children())
            for (int e = 0; e < patch.size(); e++)
              if (patch[e] == child) {
                patch[e] = patch.back();
                patch.resize(patch.size() - 1);
                break;
              }
          // .. and update it with the parent elements.
          patch.emplace_back(elem);
        }
      }
    }
  }

  row_mat.resize(idx);

  // Unset the data stored in the vertices.
  for (auto nv : vertices) nv->reset_data();
}

template <typename ForwardOp>
void MultigridPreconditioner<ForwardOp>::ApplySingleScale(
    Eigen::VectorXd &rhs) const {
  // Shortcut.
  const uint V = triang_.V;

  // Reuse a static variable for storing the corrections.
  static std::vector<double> e;
  e.reserve(V * 3);

  // Reuse a static variable for storing the residual in the downard cycle.
  static std::vector<double> r_down;
  r_down.reserve(V * 3);

  // Initialize the multigrid matrix (row_mat).
  InitializeMultigridMatrix();

  // Take zero vector as initial guess.
  Eigen::VectorXd u = Eigen::VectorXd::Zero(V);
  Eigen::VectorXd residual;

  // Do a V-cycle.
  for (size_t cycle = 0; cycle < opts_.mg_cycles; cycle++) {
    // Part 1: Down-cycle, calculates corrections while coarsening.
    {
      // Initialize the residual vector with  a(f, \Phi) - a(u, \Phi).
      residual = u;
      forward_op_.ApplySingleScale(residual);
      residual = rhs - residual;

      // Store all the corrections found in this downward cycle in a vector.
      e.clear();
      r_down.clear();

      // Step 1: Do a down-cycle and calculate 3 corrections per level.
      uint idx = 0;
      for (int k = triang_.J; k >= 1; --k) {
        for (uint vi : vertices_relaxation[k]) {
          // Calculate a(phi_vi, phi_vi).
          double a_phi_vi_phi_vi = 0;
          for (const auto &[vj, val] : row_mat[idx])
            if (vj == vi) a_phi_vi_phi_vi += val;

          // Calculate the correction in phi_vi.
          const double e_vi = residual[vi] / a_phi_vi_phi_vi;

          // Add this correction to our estimate.
          e.emplace_back(e_vi);

          // Store corrected residual.
          r_down.push_back(residual[vi]);

          // Update the residual with this new correction  a(e_vi, \cdot).
          for (const auto &[vj, val] : row_mat[idx]) residual[vj] -= e_vi * val;

          idx++;
        }

        // Restrict the residual calculated thus far.
        for (uint vertex = triang_.VerticesPerLevel(k);
             vertex < triang_.VerticesPerLevel(k + 1); vertex++)
          Restrict(vertex, residual);
      }
    }

    // Part 2:  Do exact solve on coarest level.
    Eigen::VectorXd e_0 = residual.head(triang_.InitialVertices());
    initial_triang_solver_.ApplySingleScale(e_0);

    // Create vector that will contain corrections in single scale basis.
    Eigen::VectorXd e_SS = Eigen::VectorXd::Zero(V);
    e_SS.head(triang_.InitialVertices()) = e_0;

    // Part 3: do an upward cycle
    {
      // Step 1: Walk back up and do 1-dimensional corrections.
      uint idx = row_mat.size() - 1;
      for (int k = 1; k <= triang_.J; k++) {
        // Prolongate the current correction to the next level.
        for (uint vertex = triang_.VerticesPerLevel(k);
             vertex < triang_.VerticesPerLevel(k + 1); vertex++)
          Prolongate(vertex, e_SS);

        // Step 2: Calculate corrections for these vertices.
        for (uint vi : boost::adaptors::reverse(vertices_relaxation[k])) {
          // Add the correction vi found in the downward cycle.
          e_SS[vi] += e[idx];

          // Calculate a(phi_vi, phi_vi) and a(e_SS, phi_vi).
          double a_phi_vi_phi_vi = 0;
          double a_e_phi_vi = 0;
          for (const auto &[vj, val] : row_mat[idx]) {
            // Calculate the inner product with itself.
            if (vj == vi) a_phi_vi_phi_vi += 1 * val;

            // Calculate the inner product between e and phi_vi.
            a_e_phi_vi += e_SS[vj] * val;
          }

          // Calculate the correction in phi_vi.
          const double e_i = (r_down[idx] - a_e_phi_vi) / a_phi_vi_phi_vi;

          // Add this correction
          e_SS[vi] += e_i;

          idx--;
        }
      }
    }

    // Part 3: Update approximation.
    u += e_SS;
  }

  // Finally: set the approximation as result.
  rhs = u;
}

template <template <typename> class InverseOp>
XPreconditionerOperator<InverseOp>::XPreconditionerOperator(
    const TriangulationView &triang, OperatorOptions opts)
    : BackwardOperator(triang, opts),
      stiff_op_(triang, opts),
      inverse_op_(triang, opts) {}

template <template <typename> class InverseOp>
void XPreconditionerOperator<InverseOp>::ApplySingleScale(
    Eigen::VectorXd &vec_SS) const {
  inverse_op_.ApplySingleScale(vec_SS);
  stiff_op_.ApplySingleScale(vec_SS);
  inverse_op_.ApplySingleScale(vec_SS);
}

// Explicit specializations.
template class ForwardOperator<MassOperator>;
template class ForwardOperator<StiffnessOperator>;
template class ForwardOperator<StiffPlusScaledMassOperator>;

template class DirectInverse<MassOperator>;
template class DirectInverse<StiffnessOperator>;
template class DirectInverse<StiffPlusScaledMassOperator>;

template class MultigridPreconditioner<MassOperator>;
template class MultigridPreconditioner<StiffnessOperator>;
template class MultigridPreconditioner<StiffPlusScaledMassOperator>;

template class XPreconditionerOperator<DirectInverse>;
template class XPreconditionerOperator<MultigridPreconditioner>;

}  // namespace space


// Copyright (c) 2018-2019 The Unit-e developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include <blockchain/blockchain_parameters.h>
#include <test/test_unite.h>
#include <boost/test/unit_test.hpp>

using namespace blockchain;

BOOST_FIXTURE_TEST_SUITE(blockchain_parameters_tests, ReducedTestingSetup)

BOOST_AUTO_TEST_CASE(reward_function_test) {

  Parameters params = blockchain::Parameters::TestNet();

  CAmount block_reward = params.reward_function(params, 0);
  BOOST_CHECK_EQUAL(61810000, block_reward);

  const int64_t expected_height_after_50_years = 50 * 60 * 60 * 24 * 365 / params.block_time_seconds;

  block_reward = params.reward_function(params, expected_height_after_50_years + 1);
  BOOST_CHECK_EQUAL(61810000, block_reward);
}

class ActiveChainWithTime : public ChainAccess {
 public:
  ActiveChainWithTime(const CBlock &genesis) {
    m_chain.emplace_back(MakeUnique<CBlockIndex>(genesis));
  }

  const CBlockIndex *AtDepth(const Depth depth) const override {
    return m_chain[m_chain.size() - depth].get();
  }

  const CBlockIndex *AtHeight(const Height height) const override {
    return m_chain[height].get();
  }

  void Append(const Difficulty difficulty,
              const Time time_taken_to_mine) {

    const CBlockIndex *prev_index = m_chain.back().get();

    auto index = MakeUnique<CBlockIndex>();
    index->nBits = difficulty;
    index->nTime = prev_index->nTime + time_taken_to_mine;

    m_chain.emplace_back(std::move(index));
  }

 private:
  std::vector<std::unique_ptr<CBlockIndex>> m_chain;
};

BOOST_AUTO_TEST_CASE(generic_difficulty_function_test) {

  Parameters params = Parameters::TestNet();
  params.difficulty_adjustment_window = 128;

  const CBlock &genesis = params.genesis_block.block;

  ActiveChainWithTime chain(genesis);

  Height h = 0;

  BOOST_CHECK_EQUAL(genesis.nBits, params.difficulty_function(params, h, chain));

  {
    // Ideal block time => no change in difficulty
    ++h;
    const Difficulty difficulty_before = params.difficulty_function(params, h, chain);
    for (; h < 250; ++h) {
      chain.Append(params.difficulty_function(params, h, chain), params.block_time_seconds);
    }
    const Difficulty difficulty_after = params.difficulty_function(params, h, chain);
    BOOST_CHECK_EQUAL(difficulty_before, difficulty_after);
  }

  {
    // block time decreases => difficulty value should decrease
    const Difficulty difficulty_before = params.difficulty_function(params, h, chain);
    for (; h < 500; ++h) {
      chain.Append(params.difficulty_function(params, h, chain), params.block_time_seconds - 1);
    }
    const Difficulty difficulty_after = params.difficulty_function(params, h, chain);
    BOOST_CHECK_LT(difficulty_after, difficulty_before);
  }

  {
    // Ideal block time => difficulty should not change
    // However need to give it time to settle
    for (; h < 1000; ++h) {
      chain.Append(params.difficulty_function(params, h, chain), params.block_time_seconds);
    }

    const Difficulty difficulty_before = params.difficulty_function(params, h, chain);
    for (; h < 1250; ++h) {
      chain.Append(params.difficulty_function(params, h, chain), params.block_time_seconds);
    }

    const Difficulty difficulty_after = params.difficulty_function(params, h, chain);
    BOOST_CHECK_EQUAL(difficulty_before, difficulty_after);
  }

  {
    // block time increases => difficulty value should increase
    const Difficulty difficulty_before = params.difficulty_function(params, h, chain);
    for (; h < 1500; ++h) {
      chain.Append(params.difficulty_function(params, h, chain), params.block_time_seconds + 1);
    }
    const Difficulty difficulty_after = params.difficulty_function(params, h, chain);
    BOOST_CHECK_GT(difficulty_after, difficulty_before);
  }
}

BOOST_AUTO_TEST_CASE(difficulty_function_max_test) {

  const uint256 max_difficulty_value = uint256S("000000ffffffffffffffffffffffffffffffffffffffffffffffffffffffffff");
  const arith_uint256 almost_max_difficulty_value = UintToArith256(max_difficulty_value) - 1;

  Parameters params = Parameters::TestNet();
  params.max_difficulty_value = max_difficulty_value;
  CBlock &genesis = params.genesis_block.block;
  genesis.nBits = almost_max_difficulty_value.GetCompact();

  ActiveChainWithTime chain(genesis);

  Height h = 1;
  // During the window should return difficulty of the genesis no matter what
  for (; h < params.difficulty_adjustment_window; ++h) {
    const Difficulty new_difficulty = params.difficulty_function(params, h, chain);
    chain.Append(new_difficulty, params.block_time_seconds * 2);
    BOOST_CHECK_EQUAL(almost_max_difficulty_value.GetCompact(), new_difficulty);
  }

  // After window, should adjust difficulty and hit the max value
  for (; h < 2 * params.difficulty_adjustment_window; ++h) {
    const Difficulty new_difficulty = params.difficulty_function(params, h, chain);
    chain.Append(new_difficulty, params.block_time_seconds * 2);
    BOOST_CHECK_EQUAL(UintToArith256(max_difficulty_value).GetCompact(), new_difficulty);
  }
}

BOOST_AUTO_TEST_SUITE_END()


/*
 * Copyright 2020 California  Institute  of Technology (“Caltech”)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     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.
 */

#include <x/vio/solar_update.h>
#include <x/vio/tools.h>
#include <x/ekf/state.h>
#include <boost/math/distributions.hpp>

using namespace x;
using namespace Eigen;

SolarUpdate::SolarUpdate(const x::SunAngleMeasurement& angle,
                         const x::Quaternion& quat,
                         const MatrixXd& cov_s)
{
  // Initialize Kalman update matrices
  const size_t cols = cov_s.cols();
  jac_ = MatrixXd::Zero(2, cols);
  cov_m_diag_ = VectorXd::Ones(2);
  res_ = MatrixXd::Zero(2, 1);

  // Construct update
  processSunAngle(angle, quat);
}

void SolarUpdate::processSunAngle(const x::SunAngleMeasurement& angle,
                                  const x::Quaternion& quat)
{
  //========================
  // Calibration parameters
  //========================
  // TODO(Harel) Import these from param file

  // Sun angle measurement noise (variance in deg^2)
  const double var_sun_angle = 10000 * 0.01777777777;

  // Sun sensor orientation relative to IMU
  x::Quaternion S_q_I(0.360346005598587, -0.063338979194957, 0.007502445522018, 0.930635612981541);

  // Sun vector in world frames (should be calibrated before each flight)
  // z+ points to zenith, x+ points north
  Vector3d G_sun(-0.29385515271891938, -0.55080445540063927, 0.78119370269565391);
  G_sun.normalize();

  //==========
  // Residual
  //==========
  Vector2d angles(angle.x_angle, angle.y_angle);
  Vector3d S_sun_hat = S_q_I.normalized().toRotationMatrix().transpose() *
                              quat.normalized().toRotationMatrix().transpose() *
                              G_sun;
  S_sun_hat.normalize();
  constexpr double RAD2DEG = 57.2957795130;
  Vector2d angles_hat(RAD2DEG * std::atan2(S_sun_hat(0), S_sun_hat(2)),
                             RAD2DEG * std::atan2(S_sun_hat(1), S_sun_hat(2)));

  res_ = angles - angles_hat;

  //============================
  // Measurement Jacobian matrix
  //============================

  MatrixXd mat(MatrixXd::Zero(2, 3));
  mat(0, 0) = S_sun_hat(2) / (std::pow(S_sun_hat(0), 2) + std::pow(S_sun_hat(2), 2));
  mat(1, 1) = S_sun_hat(2) / (std::pow(S_sun_hat(1), 2) + std::pow(S_sun_hat(2), 2));
  mat(0, 2) = -S_sun_hat(0) / (std::pow(S_sun_hat(0), 2) + std::pow(S_sun_hat(2), 2));
  mat(1, 2) = -S_sun_hat(1) / (std::pow(S_sun_hat(1), 2) + std::pow(S_sun_hat(2), 2));

  Vector3d skew_vector = quat.normalized().toRotationMatrix().transpose() * G_sun;

  SolarUpdate::SunAngleJacobian J_attitude = RAD2DEG * mat * S_q_I.normalized().toRotationMatrix().transpose() *
                                        x::Skew(skew_vector(0), skew_vector(1), skew_vector(2)).matrix;

  jac_.block<2, kJacCols>(0, kIdxQ) = J_attitude;

  //==================
  // Noise covariance
  //==================

  cov_m_diag_ = var_sun_angle * VectorXd::Ones(2);
}


#pragma once

#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <basalt/optimization/accumulator.h>
#include <basalt/vi_estimator/landmark_database.h>
#include <basalt/linearization/block_diagonal.hpp>

namespace basalt {

template <class Scalar>
struct RelPoseLin {
  using Mat4 = Eigen::Matrix<Scalar, 4, 4>;
  using Mat6 = Eigen::Matrix<Scalar, 6, 6>;

  Mat4 T_t_h;
  Mat6 d_rel_d_h;
  Mat6 d_rel_d_t;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

template <typename Scalar>
class LandmarkBlock {
 public:
  struct Options {
    // use Householder instead of Givens for marginalization
    bool use_householder = true;

    // use_valid_projections_only: if true, set invalid projection's
    // residual and jacobian to 0; invalid means z <= 0
    bool use_valid_projections_only = true;

    // if > 0, use huber norm with given threshold, else squared norm
    Scalar huber_parameter = 0;

    // Standard deviation of reprojection error to weight visual measurements
    Scalar obs_std_dev = 1;

    // ceres uses 1.0 / (1.0 + sqrt(SquaredColumnNorm))
    // we use 1.0 / (eps + sqrt(SquaredColumnNorm))
    Scalar jacobi_scaling_eps = 1e-6;
  };

  enum State {
    Uninitialized = 0,
    Allocated,
    NumericalFailure,
    Linearized,
    Marginalized
  };

  using Vec2 = Eigen::Matrix<Scalar, 2, 1>;
  using Vec3 = Eigen::Matrix<Scalar, 3, 1>;
  using Vec4 = Eigen::Matrix<Scalar, 4, 1>;

  using VecX = Eigen::Matrix<Scalar, Eigen::Dynamic, 1>;

  using Mat36 = Eigen::Matrix<Scalar, 3, 6>;

  using MatX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
  using RowMatX =
      Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;

  using RowMat3 = Eigen::Matrix<Scalar, 3, 3, Eigen::RowMajor>;

  virtual ~LandmarkBlock(){};

  virtual bool isNumericalFailure() const = 0;
  virtual void allocateLandmark(
      Keypoint<Scalar>& lm,
      const Eigen::aligned_unordered_map<std::pair<TimeCamId, TimeCamId>,
                                         RelPoseLin<Scalar>>& relative_pose_lin,
      const Calibration<Scalar>& calib, const AbsOrderMap& aom,
      const Options& options,
      const std::map<TimeCamId, size_t>* rel_order = nullptr) = 0;

  // may set state to NumericalFailure --> linearization at this state is
  // unusable. Numeric check is only performed for residuals that were
  // considered to be used (valid), which depends on
  // use_valid_projections_only setting.
  virtual Scalar linearizeLandmark() = 0;
  virtual void performQR() = 0;

  // Sets damping and maintains upper triangular matrix for landmarks.
  virtual void setLandmarkDamping(Scalar lambda) = 0;

  // lambda < 0 means computing exact model cost change
  virtual void backSubstitute(const VecX& pose_inc, Scalar& l_diff) = 0;

  virtual void addQ2JpTQ2Jp_mult_x(VecX& res, const VecX& x_pose) const = 0;

  virtual void addQ2JpTQ2r(VecX& res) const = 0;

  virtual void addJp_diag2(VecX& res) const = 0;

  virtual void addQ2JpTQ2Jp_blockdiag(
      BlockDiagonalAccumulator<Scalar>& accu) const = 0;

  virtual void scaleJl_cols() = 0;
  virtual void scaleJp_cols(const VecX& jacobian_scaling) = 0;
  virtual void printStorage(const std::string& filename) const = 0;
  virtual State getState() const = 0;

  virtual size_t numReducedCams() const = 0;

  virtual void get_dense_Q2Jp_Q2r(MatX& Q2Jp, VecX& Q2r,
                                  size_t start_idx) const = 0;

  virtual void get_dense_Q2Jp_Q2r_rel(
      MatX& Q2Jp, VecX& Q2r, size_t start_idx,
      const std::map<TimeCamId, size_t>& rel_order) const = 0;

  virtual void add_dense_H_b(DenseAccumulator<Scalar>& accum) const = 0;

  virtual void add_dense_H_b(MatX& H, VecX& b) const = 0;

  virtual void add_dense_H_b_rel(
      MatX& H_rel, VecX& b_rel,
      const std::map<TimeCamId, size_t>& rel_order) const = 0;

  virtual const Eigen::PermutationMatrix<Eigen::Dynamic>& get_rel_permutation()
      const = 0;

  virtual Eigen::PermutationMatrix<Eigen::Dynamic> compute_rel_permutation(
      const std::map<TimeCamId, size_t>& rel_order) const = 0;

  virtual void add_dense_H_b_rel_2(MatX& H_rel, VecX& b_rel) const = 0;

  virtual TimeCamId getHostKf() const = 0;

  virtual size_t numQ2rows() const = 0;

  // factory method
  template <int POSE_SIZE>
  static std::unique_ptr<LandmarkBlock<Scalar>> createLandmarkBlock();
};

}  // namespace basalt


/* @copyright The code is licensed under the MIT License
 *            <https://opensource.org/licenses/MIT>,
 *            Copyright (c) 2020 Christian Eskil Vaugelade Berg
 * @author Christian Eskil Vaugelade Berg
*/
#pragma once
#include <Eigen/Dense>
#include <orient/detail/skew_symmetric.hpp>
#include <orient/axis.hpp>

namespace orient::detail {

template<Axis a1, Axis a2>
constexpr Axis missing_v = Axis{3 - asIndex<a1> - asIndex<a2>};

template<Axis a1, Axis a2, Axis a3>
constexpr bool isTaitBryan_v =(asIndex<a1> + asIndex<a2> + asIndex<a3>) == 3;

template<Axis a1, Axis a2, Axis a3>
constexpr bool isProperEuler_v = (a1 == a3) and (a1 != a2);

template<Axis a1, Axis a2, Axis a3>
constexpr bool isMalformed_v = (a1 == a2) or (a2 == a3);

template<Axis a1, Axis a2, Axis a3>
struct TaitBryanTraits
{
  static const Eigen::Matrix3d sign_matrix;
  static constexpr auto a2s = asIndex<a1> + asIndex<a3>*3;
  static constexpr auto a1s = asIndex<a2> + asIndex<a3>*3;
  static constexpr auto a1c = asIndex<a3> + asIndex<a3>*3;
  static constexpr auto a3s = asIndex<a1> + asIndex<a2>*3;
  static constexpr auto a3c = asIndex<a1> + asIndex<a1>*3;

  struct GimbalLock {
    static constexpr auto a1s = asIndex<a2> + asIndex<a1>*3;
    static constexpr auto a1c = asIndex<a2> + asIndex<a2>*3;
  };
};

template<Axis a1, Axis a2, Axis a3>
const Eigen::Matrix3d TaitBryanTraits<a1,a2,a3>::sign_matrix = Eigen::Matrix3d::Identity()+orient::detail::skewSymmetric(Eigen::Vector3d::Ones());

template<Axis a1, Axis a2>
struct ProperEulerTraits
{
  static const Eigen::Matrix3d sign_matrix;
  static constexpr Axis ma = missing_v<a1,a2>;

  static constexpr auto a2c = asIndex<a1> + asIndex<a1>*3;
  static constexpr auto a1s = asIndex<a2> + asIndex<a1>*3;
  static constexpr auto a1c = asIndex<ma> + asIndex<a1>*3;
  static constexpr auto a3s = asIndex<a1> + asIndex<a2>*3;
  static constexpr auto a3c = asIndex<a1> + asIndex<ma>*3;

  struct GimbalLock {
    static constexpr auto a1s =  asIndex<a2> + asIndex<ma>*3;
    static constexpr auto a1c =  asIndex<a2> + asIndex<a2>*3;
  };
};

template<Axis a1, Axis a2>
const Eigen::Matrix3d ProperEulerTraits<a1,a2>::sign_matrix = Eigen::Matrix3d::Identity()+orient::detail::skewSymmetric(Eigen::Vector3d::Ones());

}


//==============================================================================
//         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 BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_REMQUO_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_REMQUO_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/remquo.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/simd/include/functions/simd/round.hpp>
#include <boost/simd/include/functions/simd/tofloat.hpp>
#include <boost/simd/include/functions/simd/toint.hpp>
#include <boost/simd/include/functions/simd/is_gtz.hpp>
#include <boost/simd/include/functions/simd/is_eqz.hpp>
#include <boost/simd/include/functions/simd/if_allbits_else.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>
#include <boost/mpl/logical.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::remquo_, tag::cpu_,
                             (A0)(X),
                             ((simd_<arithmetic_<A0>,X>))
                             ((simd_<arithmetic_<A0>,X>))
                             )
  {
    typedef typename dispatch::meta::as_floating<A0>::type               stA0;
    typedef typename dispatch::meta::as_integer<A0, signed>::type     iA0;
    typedef boost::fusion::tuple<stA0,iA0>          result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
      {
        result_type res;
        boost::simd::remquo( tofloat(a0), tofloat(a1)
              , boost::fusion::at_c<0>(res),  boost::fusion::at_c<1>(res)
              );
        return res;
      }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF(  boost::simd::tag::remquo_, tag::cpu_,(A0)(A1)(A2)(A3)(X)
                                  ,( boost::mpl::and_ <
                                     boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                          , boost::simd::meta::cardinal_of<A1> >,
                                     boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                          , boost::simd::meta::cardinal_of<A2> >,
                                     boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                          , boost::simd::meta::cardinal_of<A3> >
                                     >
                                  )
                               , ((simd_<arithmetic_<A0>,X>))
                                 ((simd_<arithmetic_<A1>,X>))
                                 ((simd_<floating_<A2>,X>))
                                 ((simd_<integer_<A3>,X>))
                             )
  {
    typedef void result_type;
    inline result_type operator()(A0 const& a0, A1 const& a1,
                                  A2 & a2, A3 & a3) const
    {
      boost::simd::remquo(tofloat(a0), tofloat(a1), a2, a3);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF(  boost::simd::tag::remquo_, tag::cpu_,(A0)(A1)(X)
                                  ,( boost::mpl::equal_to < boost::simd::meta::cardinal_of<A0>
                                                          , boost::simd::meta::cardinal_of<A1> >)
                                  , ((simd_<floating_<A0>,X>))
                                  ((simd_<floating_<A0>,X>))
                                  ((simd_<floating_<A0>,X>))
                                  ((simd_<integer_<A1>,X>))
                             )
  {
    typedef void result_type;
    inline result_type operator()(A0 const& a0, A0 const& a1,
                                  A0 & a2, A1 & a3) const
    {
      a2 = round(a0/a1);
      a3 = toint(a2);
      a2 =  if_nan_else(l_or(is_invalid(a0), is_eqz(a1)), a0-a2*a1);
    }
  };
} } }
#endif


#include <iostream>
#include <fstream>
#include <map>
#include <algorithm>

#include <CGAL/Delaunay_triangulation_2.h>
#include <CGAL/Simple_cartesian.h>
#include <CGAL/point_generators_2.h>

#include <CGAL/Timer.h>
#include<boost/range/iterator_range.hpp>
#include <boost/unordered_map.hpp>
#include <unordered_map>

#include <boost/random/random_number_generator.hpp>
#include <boost/random/linear_congruential.hpp>

typedef CGAL::Simple_cartesian<double>  Kernel;
typedef Kernel::Point_2                 Point_2;
typedef Kernel::Vector_2                Vector_2;
typedef CGAL::Creator_uniform_2<double,Point_2>  Pt_creator;
typedef CGAL::Random_points_in_disc_2<Point_2,Pt_creator>  Random_points;;
typedef CGAL::Delaunay_triangulation_2<Kernel> Dt;
typedef Dt::Vertex_handle Vertex_handle;
typedef Dt::Finite_vertices_iterator Finite_vertices_iterator;

typedef CGAL::Timer                     Timer;

void  fct(int ii, int jj)
{
  typedef std::map<Vertex_handle,Point_2> SM;
  typedef std::unordered_map<Vertex_handle,Point_2> SUM;
  typedef boost::unordered_map<Vertex_handle,Point_2> BUM;

  Dt dt;
  Vector_2 v(0,0);
  Random_points rp( 250);
  std::vector<Point_2> points;
  for(int i =0; i < ii; i++){
    Point_2 p = *rp++;
    points.push_back(p);
  }

  dt.insert(points.begin(), points.end());

  std::vector<Vertex_handle> vertices;
  Finite_vertices_iterator b = dt.finite_vertices_begin(), e = dt.finite_vertices_end();
  for(; b!=e; ++b){
    vertices.push_back(b);
  }
  random_shuffle(vertices.begin(), vertices.end());

  Timer tsmc, tsumc, tbumc;
  Timer tsmq, tsumq, tbumq;
  for(int j=0; j <jj; j++){

    {
      tsmc.start();
      SM sm;
      for(Vertex_handle vh : vertices){
        sm[vh] = vh->point();
      }
      tsmc.stop();

      tsmq.start();
      for(Vertex_handle vh : vertices){
        v = v + (sm[vh] - CGAL::ORIGIN);
      }
      tsmq.stop();
    }
    {
      tsumc.start();
      SUM sm;
      for(Vertex_handle vh : vertices){
        sm[vh] = vh->point();
      }
      tsumc.stop();

      tsumq.start();
      for(Vertex_handle vh : vertices){
        v = v + (sm[vh] - CGAL::ORIGIN);
      }
      tsumq.stop();
    }
    {
      tbumc.start();
      BUM sm;
      for(Vertex_handle vh : vertices){
        sm[vh] = vh->point();
      }
      tbumc.stop();

      tbumq.start();
      for(Vertex_handle vh : vertices){
        v = v + (sm[vh] - CGAL::ORIGIN);
      }
      tbumq.stop();
    }
  }
  std::cerr << ii << " items and queries (repeated " << jj << " times)" << std::endl;
  std::cerr << "std::map             construction : "<< tsmc.time() << " sec." << std::endl;
  std::cerr << "std::map             queries      : "<< tsmq.time() << " sec." << std::endl;

  std::cerr << "std::unordered_map   construction : "<< tsumc.time() << " sec." << std::endl;
  std::cerr << "std::unordered_map   queries      : "<< tsumq.time() << " sec." << std::endl;

  std::cerr << "boost::unordered_map construction : "<< tbumc.time() << " sec." << std::endl;
  std::cerr << "boost::unordered_map queries      : "<< tbumq.time() << " sec.\n" << std::endl;
}

int main(int , char* argv[])
{
  fct(1000000, 10);
  fct(100000, 100);
  fct(10000, 1000);
  fct(1000, 10000);
  fct(500,  20000);
  fct(250,  40000);
  fct(100, 100000);
  return 0;
}


#include<cstddef>
#include "constants.h"
// common C head files used in the program
#include<cstdlib>
#include<cstdio>

// external math functions
#include<cmath>
#include <boost/math/special_functions/gamma.hpp>
#include<string>
#include<vector>
#include<iostream>
#include<fstream>
#include<iomanip>
#include<string>
#include<list>
#include<set>
#include<map>
#include<iterator>
#include<algorithm>
#define THRESHOLD_MATH  0.000000000000001E0
using namespace std;
using namespace boost::math;
typedef size_t        UInt;
typedef double        Double;

Double fm(const Double& a, const UInt& m);

void newSSSS(const UInt& maxM, const UInt& inp2, const Double* iexp, const Double* icoe,
		const Double* ifac,
		const UInt& jnp2, const Double* jexp, const Double* jcoe, const Double* jfac,
		const Double* A, const Double* B, const Double* P,
		const Double* C, const Double* D, const Double* Q, Double* ssss);

void oldSSSS(const UInt& maxM, const UInt& iNP2, const Double* iCoe, const Double* iExp, const Double* A, const Double* B, const UInt& jNP2, const Double* jCoe, const Double* jExp, const Double* C, const Double* D);

Double fm(const Double& a, const UInt& m) {
	Double x = 1.0E0;
	if (fabs(a)<1.0E-14) return (pow(x,2*m+1)/(2*m+1));
	Double f12 = 1.0E0/2.0E0;
	Double p   = 1.0E0/(2.0E0*pow(a,m+f12));
	Double upperLimit = a*pow(x,2.0E0);
	return tgamma_lower(m+f12,upperLimit)*p;
}

void newSSSS(const UInt& maxM, const UInt& inp2, const Double* iexp, const Double* icoe,
		const Double* ifac,
		const UInt& jnp2, const Double* jexp, const Double* jcoe, const Double* jfac,
		const Double* A, const Double* B, const Double* P,
		const Double* C, const Double* D, const Double* Q)
{

	vector<Double> result(maxM+1,0.0E0);
	for(UInt ip2=0; ip2<inp2; ip2++) {
		Double onedz = iexp[ip2];
		Double ic2   = icoe[ip2];
		Double fbra  = ifac[ip2];
		Double oned2z= 0.5E0*onedz;
		UInt offsetP = 3*ip2;
		Double PX    = P[offsetP  ];
		Double PY    = P[offsetP+1];
		Double PZ    = P[offsetP+2];
		Double PAX   = PX - A[0];
		Double PAY   = PY - A[1];
		Double PAZ   = PZ - A[2];
		Double PBX   = PX - B[0];
		Double PBY   = PY - B[1];
		Double PBZ   = PZ - B[2];
		for(UInt jp2=0; jp2<jnp2; jp2++) {
			Double onede = jexp[jp2];
			Double jc2   = jcoe[jp2];
			Double fket  = jfac[jp2];
			Double oned2e= 0.5E0*onedz;
			UInt offsetQ= 3*jp2;
			Double QX    = Q[offsetQ  ];
			Double QY    = Q[offsetQ+1];
			Double QZ    = Q[offsetQ+2];
			Double QCX   = QX - C[0];
			Double QCY   = QY - C[1];
			Double QCZ   = QZ - C[2];
			Double QDX   = QX - D[0];
			Double QDY   = QY - D[1];
			Double QDZ   = QZ - D[2];
			Double rho   = 1.0E0/(onedz+onede);
			Double sqrho = sqrt(rho);
			Double WX    = rho*(PX*onede + QX*onedz);
			Double WY    = rho*(PY*onede + QY*onedz);
			Double WZ    = rho*(PZ*onede + QZ*onedz);
			Double oned2k= 0.5E0*rho*onede*onedz;
			Double WPX   = WX - PX;
			Double WPY   = WY - PY;
			Double WPZ   = WZ - PZ;
			Double rhod2zsq = rho*oned2z*onedz;
			Double WQX   = WX - QX;
			Double WQY   = WY - QY;
			Double WQZ   = WZ - QZ;
			Double rhod2esq = rho*oned2e*onede;
			Double prefactor = ic2*jc2*fbra*fket;
			Double PQ2   = (PX-QX)*(PX-QX)+(PY-QY)*(PY-QY)+(PZ-QZ)*(PZ-QZ);
			Double u     = rho*PQ2;
			Double squ   = sqrt(u);

			// here we need to set the ssss integrals
			Double I_SSSS_M0  = 0.0E0;
			Double I_SSSS_M1  = 0.0E0;
			Double I_SSSS_M2  = 0.0E0;
			Double I_SSSS_M3  = 0.0E0;
			Double I_SSSS_M4  = 0.0E0;
			Double I_SSSS_M5  = 0.0E0;
			Double I_SSSS_M6  = 0.0E0;
			Double I_SSSS_M7  = 0.0E0;
			Double I_SSSS_M8  = 0.0E0;
			Double I_SSSS_M9  = 0.0E0;
			Double I_SSSS_M10 = 0.0E0;
			Double I_SSSS_M11 = 0.0E0;

			//
			// we use this way to generate the (SS|SS)^{m}
			// maxM=0, we just use erf function
			// if maxM<=10 and maxM>0, then mainly we use up recursive relation
			// else we will calculate fm directly and use down recursive relation
			// see the doc for more information
			//
			if (maxM==0) {
				I_SSSS_M0 = (prefactor*sqrho/squ)*erf(squ);
				//cout << "new ssss prefactor " << (prefactor*sqrho)*TWOOVERSQRTPI << endl;
				result[0] += I_SSSS_M0;
			}else if (maxM<=10) {

				//
				// now here for maxM<=10 and maxM>0
				// we also have two situations
				// 1  if u <=1; use power series to get f_{max}(u), then go down recursive way;
				// 2  if u > 1; calculate erf(u), then go up recursive way;
				//

				if (u<=1.0E0) {

					// calculate (SS|SS)^{Mmax}
					Double u2  = 2.0E0*u;
					I_SSSS_M8  = 1.0E0+u2*ONEOVER41;
					I_SSSS_M8  = 1.0E0+u2*ONEOVER39*I_SSSS_M8;
					I_SSSS_M8  = 1.0E0+u2*ONEOVER37*I_SSSS_M8;
					I_SSSS_M8  = 1.0E0+u2*ONEOVER35*I_SSSS_M8;
					I_SSSS_M8  = 1.0E0+u2*ONEOVER33*I_SSSS_M8;
					I_SSSS_M8  = 1.0E0+u2*ONEOVER31*I_SSSS_M8;
					I_SSSS_M8  = 1.0E0+u2*ONEOVER29*I_SSSS_M8;
					I_SSSS_M8  = 1.0E0+u2*ONEOVER27*I_SSSS_M8;
					I_SSSS_M8  = 1.0E0+u2*ONEOVER25*I_SSSS_M8;
					I_SSSS_M8  = 1.0E0+u2*ONEOVER23*I_SSSS_M8;
					I_SSSS_M8  = 1.0E0+u2*ONEOVER21*I_SSSS_M8;
					I_SSSS_M8  = 1.0E0+u2*ONEOVER19*I_SSSS_M8;
					I_SSSS_M8  = ONEOVER17*I_SSSS_M8;
					Double eu  = exp(-u);
					Double f   = TWOOVERSQRTPI*prefactor*sqrho*eu;
					I_SSSS_M8  = f*I_SSSS_M8;

					// now use down recursive relation to get
					// rest of (SS|SS)^{m}
					I_SSSS_M7  = ONEOVER15*(u2*I_SSSS_M8+f);
					I_SSSS_M6  = ONEOVER13*(u2*I_SSSS_M7+f);
					I_SSSS_M5  = ONEOVER11*(u2*I_SSSS_M6+f);
					I_SSSS_M4  = ONEOVER9*(u2*I_SSSS_M5+f);
					I_SSSS_M3  = ONEOVER7*(u2*I_SSSS_M4+f);
					I_SSSS_M2  = ONEOVER5*(u2*I_SSSS_M3+f);
					I_SSSS_M1  = ONEOVER3*(u2*I_SSSS_M2+f);
					I_SSSS_M0  = ONEOVER1*(u2*I_SSSS_M1+f);

					result[0] += I_SSSS_M0;
					result[1] += I_SSSS_M1;
					result[2] += I_SSSS_M2;
					result[3] += I_SSSS_M3;
					result[4] += I_SSSS_M4;
					result[5] += I_SSSS_M5;
					result[6] += I_SSSS_M6;
					result[7] += I_SSSS_M7;
					result[8] += I_SSSS_M8;
				} else {

					//cout << "up recursive relation " << endl;
					// calculate (SS|SS)^{0}
					I_SSSS_M0     = (prefactor*sqrho/squ)*erf(squ);

					// now use up recursive relation
					Double oneO2u = 0.5E0/u;
					Double eu     = exp(-u);
					Double f      = TWOOVERSQRTPI*prefactor*sqrho*eu;
					I_SSSS_M1     = oneO2u*(1.0E0*I_SSSS_M0-f);
					I_SSSS_M2     = oneO2u*(3.0E0*I_SSSS_M1-f);
					I_SSSS_M3     = oneO2u*(5.0E0*I_SSSS_M2-f);
					I_SSSS_M4     = oneO2u*(7.0E0*I_SSSS_M3-f);
					I_SSSS_M5     = oneO2u*(9.0E0*I_SSSS_M4-f);
					I_SSSS_M6     = oneO2u*(11.0E0*I_SSSS_M5-f);
					I_SSSS_M7     = oneO2u*(13.0E0*I_SSSS_M6-f);
					I_SSSS_M8     = oneO2u*(15.0E0*I_SSSS_M7-f);

					result[0] += I_SSSS_M0;
					result[1] += I_SSSS_M1;
					result[2] += I_SSSS_M2;
					result[3] += I_SSSS_M3;
					result[4] += I_SSSS_M4;
					result[5] += I_SSSS_M5;
					result[6] += I_SSSS_M6;
					result[7] += I_SSSS_M7;
					result[8] += I_SSSS_M8;

					//
					// The algorithm below is actually not numerically
					// good. and I think erf function may not more
					// expensive than calculating O*(2m-1)!!/|PQ|
					// therefore in our codes we do not do it
					//
					/*

					//cout << "far end expansion" << endl;
					// calculate (SS|SS)^{Mmax} through O*(2m-1)!!/|PQ|
					Double f0     = prefactor*sqrho;
					I_SSSS_M8     = f0/squ;
					Double oneO2u = 0.5E0/u;
					I_SSSS_M8    *= oneO2u;
					I_SSSS_M8    *= 3.0E0*oneO2u;
					I_SSSS_M8    *= 5.0E0*oneO2u;
					I_SSSS_M8    *= 7.0E0*oneO2u;
					I_SSSS_M8    *= 9.0E0*oneO2u;
					I_SSSS_M8    *= 11.0E0*oneO2u;
					I_SSSS_M8    *= 13.0E0*oneO2u;
					I_SSSS_M8    *= 15.0E0*oneO2u;

					//Double x = TWOOVERSQRTPI*prefactor*sqrho*fm(u,8);
					//cout << "difference between his I_SSSS_M8 and standard" <<
					//	fabs(I_SSSS_M8-x) << endl;

					// now use down recursive relation to get
					// rest of (SS|SS)^{m}
					Double u2     = 2.0E0*u;
					Double f      = TWOOVERSQRTPI*f0*exp(-u);
					I_SSSS_M7     = ONEOVER15*(u2*I_SSSS_M8+f);
					I_SSSS_M6     = ONEOVER13*(u2*I_SSSS_M7+f);
					I_SSSS_M5     = ONEOVER11*(u2*I_SSSS_M6+f);
					I_SSSS_M4     = ONEOVER9*(u2*I_SSSS_M5+f);
					I_SSSS_M3     = ONEOVER7*(u2*I_SSSS_M4+f);
					I_SSSS_M2     = ONEOVER5*(u2*I_SSSS_M3+f);
					I_SSSS_M1     = ONEOVER3*(u2*I_SSSS_M2+f);
					I_SSSS_M0     = u2*I_SSSS_M1+f;

					cout << "u is " << u << endl;
					Double x = TWOOVERSQRTPI*prefactor*sqrho*fm(u,0);
					cout << "difference between his I_SSSS_M0 and standard in down recursive " <<
					fabs(I_SSSS_M0-x) << endl;
					x = TWOOVERSQRTPI*prefactor*sqrho*fm(u,8);
					cout << "difference between his I_SSSS_M8 and standard in down recursive " <<
					fabs(I_SSSS_M8-x) << endl;
					x = TWOOVERSQRTPI*prefactor*sqrho*fm(u,7);
					cout << "difference between his I_SSSS_M7 and standard in down recursive " <<
					fabs(I_SSSS_M7-x) << endl;
					x = TWOOVERSQRTPI*prefactor*sqrho*fm(u,6);
					cout << "difference between his I_SSSS_M6 and standard in down recursive " <<
					fabs(I_SSSS_M6-x) << endl;
					x = TWOOVERSQRTPI*prefactor*sqrho*fm(u,5);
					cout << "difference between his I_SSSS_M5 and standard in down recursive " <<
					fabs(I_SSSS_M5-x) << endl;
					x = TWOOVERSQRTPI*prefactor*sqrho*fm(u,4);
					cout << "difference between his I_SSSS_M4 and standard in down recursive " <<
					fabs(I_SSSS_M4-x) << endl;
					x = TWOOVERSQRTPI*prefactor*sqrho*fm(u,3);
					cout << "difference between his I_SSSS_M3 and standard in down recursive " <<
					fabs(I_SSSS_M3-x) << endl;
					x = TWOOVERSQRTPI*prefactor*sqrho*fm(u,2);
					cout << "difference between his I_SSSS_M2 and standard in down recursive " <<
					fabs(I_SSSS_M2-x) << endl;
					x = TWOOVERSQRTPI*prefactor*sqrho*fm(u,1);
					cout << "difference between his I_SSSS_M1 and standard in down recursive " <<
					fabs(I_SSSS_M1-x) << endl;

					result[0] += I_SSSS_M0;
					result[1] += I_SSSS_M1;
					result[2] += I_SSSS_M2;
					result[3] += I_SSSS_M3;
					result[4] += I_SSSS_M4;
					result[5] += I_SSSS_M5;
					result[6] += I_SSSS_M6;
					result[7] += I_SSSS_M7;
					result[8] += I_SSSS_M8;
					*/
				}

			}else{

				// calculate (SS|SS)^{Mmax} with incomplete gamma function
				Double f0     = TWOOVERSQRTPI*prefactor*sqrho;
				I_SSSS_M11    = f0*fm(u,11);

				// now use down recursive relation to get
				// rest of (SS|SS)^{m}
				Double u2     = 2.0E0*u;
				Double f      = f0*exp(-u);
				I_SSSS_M10    = ONEOVER21*(u2*I_SSSS_M11+f);
				I_SSSS_M9     = ONEOVER19*(u2*I_SSSS_M10+f);
				I_SSSS_M8     = ONEOVER17*(u2*I_SSSS_M9+f);
				I_SSSS_M7     = ONEOVER15*(u2*I_SSSS_M8+f);
				I_SSSS_M6     = ONEOVER13*(u2*I_SSSS_M7+f);
				I_SSSS_M5     = ONEOVER11*(u2*I_SSSS_M6+f);
				I_SSSS_M4     = ONEOVER9*(u2*I_SSSS_M5+f);
				I_SSSS_M3     = ONEOVER7*(u2*I_SSSS_M4+f);
				I_SSSS_M2     = ONEOVER5*(u2*I_SSSS_M3+f);
				I_SSSS_M1     = ONEOVER3*(u2*I_SSSS_M2+f);
				I_SSSS_M0     = ONEOVER1*(u2*I_SSSS_M1+f);

				result[0] += I_SSSS_M0;
				result[1] += I_SSSS_M1;
				result[2] += I_SSSS_M2;
				result[3] += I_SSSS_M3;
				result[4] += I_SSSS_M4;
				result[5] += I_SSSS_M5;
				result[6] += I_SSSS_M6;
				result[7] += I_SSSS_M7;
				result[8] += I_SSSS_M8;
				result[9] += I_SSSS_M9;
				result[10] += I_SSSS_M10;
				result[11] += I_SSSS_M11;
			}
		}
	}
	for(UInt n=0; n<=maxM; n++) {
		printf("M is: %d  integral: %-18.14f\n", n, result[n]);
	}
}

void oldSSSS(const UInt& maxM, const UInt& iNP2, const Double* iCoe, const Double* iExp, const Double* A, const Double* B, const UInt& jNP2, const Double* jCoe, const Double* jExp, const Double* C, const Double* D)
{
	vector<Double> result(maxM+1,0.0E0);
	for(UInt ip=0; ip<iNP2; ip++) {

		Double alpha = iExp[2*ip];
		Double beta  = iExp[2*ip+1];
		Double zeta  = alpha+beta;
		Double onedz = 1.0E0/zeta;
		Double ic    = iCoe[ip];
		Double adz   = alpha*onedz;
		Double bdz   = beta*onedz;
		Double PX    = A[0]*adz + B[0]*bdz;
		Double PY    = A[1]*adz + B[1]*bdz;
		Double PZ    = A[2]*adz + B[2]*bdz;
		Double PAX   = PX - A[0];
		Double PAY   = PY - A[1];
		Double PAZ   = PZ - A[2];
		Double pidz = PI*onedz;
		Double I_S_S_bra = ic*pow(pidz,1.5E0);
		Double oned2z = 0.5E0*onedz;
		Double oned2zsq  = oned2z*onedz;
		for(UInt jp=0; jp<jNP2; jp++) {

			Double gamma = jExp[2*jp];
			Double delta = jExp[2*jp+1];
			Double eta   = gamma+delta;
			Double onede = 1.0E0/eta;
			Double jc    = jCoe[jp];
			Double gde   = gamma*onede;
			Double dde   = delta*onede;
			Double QX    = C[0]*gde + D[0]*dde;
			Double QY    = C[1]*gde + D[1]*dde;
			Double QZ    = C[2]*gde + D[2]*dde;
			Double QCX   = QX - C[0];
			Double QCY   = QY - C[1];
			Double QCZ   = QZ - C[2];
			Double PQX   = PX - QX;
			Double PQY   = PY - QY;
			Double PQZ   = PZ - QZ;
			Double PQ2   = PQX*PQX+PQY*PQY+PQZ*PQZ;
			Double kappa = zeta+eta;
			Double onedk = 1.0E0/kappa;
			Double rho   = zeta*eta*onedk;
			Double zdk   = zeta*onedk;
			Double edk   = eta*onedk;
			Double WX    = PX*zdk + QX*edk;
			Double WY    = PY*zdk + QY*edk;
			Double WZ    = PZ*zdk + QZ*edk;
			Double oned2k= 0.5E0*onedk;
			Double WPX   = WX - PX;
			Double WPY   = WY - PY;
			Double WPZ   = WZ - PZ;
			Double rhod2zsq = rho*oned2zsq;
			Double WQX   = WX - QX;
			Double WQY   = WY - QY;
			Double WQZ   = WZ - QZ;
			Double oned2e   = 0.5E0*onede;
			Double oned2esq = oned2e*onede;
			Double rhod2esq = rho*oned2esq;
			Double pide  = PI*onede;
			Double rhodpi= rho/PI;
			Double factor= jc*pow(pide,1.5E0)*pow(rhodpi,0.5E0);
			Double u     = rho*PQ2;
			Double prefactor = 2.0E0*I_S_S_bra*factor;
			//cout << "old ssss prefactor " << prefactor << endl;

			for(UInt n=0; n<=maxM; n++) {
				result[n] += prefactor*fm(u,n);
			}
		}
	}
	for(UInt n=0; n<=maxM; n++) {
		printf("M is: %d  integral: %-18.14f\n", n, result[n]);
	}
}

int main() {

	//
	// lambda is used to adjust the distance betwen P and Q
	//
	Double lambda = 2.0;
	UInt ntest = 3;

	//
	// form the basic shell data - arbitrary
	//
	Double exp1[3];
	Double c1[3];
	exp1[0] = 18.7311370;
	exp1[1] = 2.8253937;
	exp1[2] = 0.6401217;
	c1[0]   = 0.03349460;
	c1[1]   = 0.23472695;
	c1[2]   = 0.81375733;
	Double A[3];
	A[0]    = 0.0;
	A[1]    = 0.0;
	A[2]    = 0.0;

	Double exp2[3];
	Double c2[3];
	exp2[0] = 7.8682724;
	exp2[1] = 1.8812885;
	exp2[2] = 0.5442493;
	c2[0]   = 0.0689991;
	c2[1]   = 0.3164240;
	c2[2]   = 0.7443083;
	Double B[3];
	B[0]    = 1.0;
	B[1]    = 0.0;
	B[2]    = 0.0;

	Double exp3[3];
	Double c3[3];
	exp3[0] = 5.8682724;
	exp3[1] = 2.8812885;
	exp3[2] = 0.7442493;
	c3[0]   = 0.0149991;
	c3[1]   = 0.6164240;
	c3[2]   = 0.5443083;
	Double C[3];
	C[0]    = 0.0;
	C[1]    = 1.0*lambda;
	C[2]    = 0.0;

	Double exp4[3];
	Double c4[3];
	exp4[0] = 8.78682724;
	exp4[1] = 4.11112885;
	exp4[2] = 0.2442493;
	c4[0]   = 0.0449991;
	c4[1]   = 0.264240;
	c4[2]   = 0.2443083;
	Double D[3];
	D[0]    = 1.0;
	D[1]    = 1.0*lambda;
	D[2]    = 0.0;

	//
	// now do the SSSS in old way
	//
	bool doOldSSSS = true;
	if (doOldSSSS) {

		// AB pairs
		UInt iNP2 = 9;
		vector<Double> iExp;
		vector<Double> iCoe;
		iExp.reserve(2*iNP2);
		iCoe.reserve(iNP2);
		Double AB2 = pow(A[0]-B[0],2)+pow(A[1]-B[1],2)+pow(A[2]-B[2],2);
		for(UInt j=0;j<3;j++) {
			for(UInt i=0;i<3;i++) {
				Double ia    = exp1[i];
				Double ja    = exp2[j];
				Double ic    = c1[i];
				Double jc    = c2[j];
				Double alpha = ia+ja;
				Double ab    = -ia*ja/alpha;
				Double pref  = exp(ab*AB2);
				Double d     = ic*jc*pref;
				iExp.push_back(ia);
				iExp.push_back(ja);
				iCoe.push_back(d);
			}
		}

		// CD pairs
		UInt jNP2 = 9;
		vector<Double> jExp;
		vector<Double> jCoe;
		jExp.reserve(2*jNP2);
		jCoe.reserve(jNP2);
		Double CD2 = pow(C[0]-D[0],2)+pow(C[1]-D[1],2)+pow(C[2]-D[2],2);
		for(UInt j=0;j<3;j++) {
			for(UInt i=0;i<3;i++) {
				Double ia    = exp3[i];
				Double ja    = exp4[j];
				Double ic    = c3[i];
				Double jc    = c4[j];
				Double alpha = ia+ja;
				Double ab    = -ia*ja/alpha;
				Double pref  = exp(ab*CD2);
				Double d     = ic*jc*pref;
				jExp.push_back(ia);
				jExp.push_back(ja);
				jCoe.push_back(d);
			}
		}

		// now do the work
		for(UInt i=0; i<ntest; i++) {

			UInt maxM = 8;
			if (i == 1) maxM = 11;
			if (i == 2) maxM = 0;
			oldSSSS(maxM,iNP2,&iCoe.front(),&iExp.front(),A,B,
					jNP2,&jCoe.front(),&jExp.front(),C,D);
		}
	}

	//
	// now do the SSSS in new way
	//
	bool donewSSSS = true;
	if (donewSSSS) {

		// AB pairs
		UInt inp2 = 9;
		vector<Double> iexp;
		vector<Double> icoe;
		vector<Double> ifac;
		vector<Double> P;
		iexp.reserve(inp2);
		icoe.reserve(inp2);
		ifac.reserve(inp2);
		P.reserve(3*inp2);
		Double AB2 = pow(A[0]-B[0],2)+pow(A[1]-B[1],2)+pow(A[2]-B[2],2);
		for(UInt j=0;j<3;j++) {
			for(UInt i=0;i<3;i++) {
				Double ia    = exp1[i];
				Double ja    = exp2[j];
				Double ic    = c1[i];
				Double jc    = c2[j];
				Double alpha = 1/(ia+ja);
				iexp.push_back(alpha);
				icoe.push_back(ic*jc);
				Double ab    = -ia*ja/(ia+ja);
				Double pref  = exp(ab*AB2);
				Double d     = pref*pow(PI*alpha,1.5E0);
				ifac.push_back(d);
				Double adz   = ia*alpha;
				Double bdz   = ja*alpha;
				Double PX    = A[0]*adz + B[0]*bdz;
				Double PY    = A[1]*adz + B[1]*bdz;
				Double PZ    = A[2]*adz + B[2]*bdz;
				P.push_back(PX);
				P.push_back(PY);
				P.push_back(PZ);
			}
		}

		// CD pairs
		UInt jnp2 = 9;
		vector<Double> jexp;
		vector<Double> jcoe;
		vector<Double> jfac;
		vector<Double> Q;
		jexp.reserve(jnp2);
		jcoe.reserve(jnp2);
		jfac.reserve(jnp2);
		Q.reserve(3*jnp2);
		Double CD2 = pow(C[0]-D[0],2)+pow(C[1]-D[1],2)+pow(C[2]-D[2],2);
		for(UInt j=0;j<3;j++) {
			for(UInt i=0;i<3;i++) {
				Double ia    = exp3[i];
				Double ja    = exp4[j];
				Double ic    = c3[i];
				Double jc    = c4[j];
				Double alpha = 1/(ia+ja);
				jexp.push_back(alpha);
				jcoe.push_back(ic*jc);
				Double ab    = -ia*ja/(ia+ja);
				Double pref  = exp(ab*CD2);
				Double d     = pref*pow(PI*alpha,1.5E0);
				jfac.push_back(d);
				Double adz   = ia*alpha;
				Double bdz   = ja*alpha;
				Double QX    = C[0]*adz + D[0]*bdz;
				Double QY    = C[1]*adz + D[1]*bdz;
				Double QZ    = C[2]*adz + D[2]*bdz;
				Q.push_back(QX);
				Q.push_back(QY);
				Q.push_back(QZ);
			}
		}

		// now do the work
		for(UInt i=0; i<ntest; i++) {

			UInt maxM = 8;
			if (i == 1) maxM = 11;
			if (i == 2) maxM = 0;
			newSSSS(maxM,inp2,&iexp.front(),&icoe.front(),&ifac.front(),
					jnp2,&jexp.front(),&jcoe.front(),&jfac.front(),A,B,&P.front(),
					C,D,&Q.front());
		}
	}
	return 0;
}


/**
 * @date Tue Jan 18 17:07:26 2011 +0100
 * @author André Anjos <andre.anjos@idiap.ch>
 *
 * @brief Implementation of MLPs
 *
 * Copyright (C) 2011-2014 Idiap Research Institute, Martigny, Switzerland
 */

#include <sys/time.h>
#include <cmath>
#include <boost/format.hpp>
#include <boost/make_shared.hpp>

#include <bob.core/check.h>
#include <bob.core/array_copy.h>
#include <bob.core/assert.h>
#include <bob.math/linear.h>

#include <bob.learn.mlp/machine.h>

bob::learn::mlp::Machine::Machine (size_t input, size_t output):
  m_input_sub(input),
  m_input_div(input),
  m_weight(1),
  m_bias(1),
  m_hidden_activation(boost::make_shared<bob::learn::activation::HyperbolicTangentActivation>()),
  m_output_activation(m_hidden_activation),
  m_buffer(1)
{
  resize(input, output);
  m_input_sub = 0;
  m_input_div = 1;
  setWeights(0);
  setBiases(0);
}

bob::learn::mlp::Machine::Machine (size_t input, size_t hidden, size_t output):
  m_input_sub(input),
  m_input_div(input),
  m_weight(2),
  m_bias(2),
  m_hidden_activation(boost::make_shared<bob::learn::activation::HyperbolicTangentActivation>()),
  m_output_activation(m_hidden_activation),
  m_buffer(2)
{
  resize(input, hidden, output);
  m_input_sub = 0;
  m_input_div = 1;
  setWeights(0);
  setBiases(0);
}

bob::learn::mlp::Machine::Machine (size_t input, const std::vector<size_t>& hidden, size_t output):
  m_input_sub(input),
  m_input_div(input),
  m_weight(hidden.size()+1),
  m_bias(hidden.size()+1),
  m_hidden_activation(boost::make_shared<bob::learn::activation::HyperbolicTangentActivation>()),
  m_output_activation(m_hidden_activation),
  m_buffer(hidden.size()+1)
{
  resize(input, hidden, output);
  m_input_sub = 0;
  m_input_div = 1;
  setWeights(0);
  setBiases(0);
}

bob::learn::mlp::Machine::Machine (const std::vector<size_t>& shape):
  m_hidden_activation(boost::make_shared<bob::learn::activation::HyperbolicTangentActivation>()),
  m_output_activation(m_hidden_activation)
{
  resize(shape);
  m_input_sub = 0;
  m_input_div = 1;
  setWeights(0);
  setBiases(0);
}

bob::learn::mlp::Machine::Machine (const bob::learn::mlp::Machine& other):
  m_input_sub(bob::core::array::ccopy(other.m_input_sub)),
  m_input_div(bob::core::array::ccopy(other.m_input_div)),
  m_weight(other.m_weight.size()),
  m_bias(other.m_bias.size()),
  m_hidden_activation(other.m_hidden_activation),
  m_output_activation(other.m_output_activation),
  m_buffer(other.m_buffer.size())
{
  for (size_t i=0; i<other.m_weight.size(); ++i) {
    m_weight[i].reference(bob::core::array::ccopy(other.m_weight[i]));
    m_bias[i].reference(bob::core::array::ccopy(other.m_bias[i]));
    m_buffer[i].reference(bob::core::array::ccopy(other.m_buffer[i]));
  }
}

bob::learn::mlp::Machine::Machine (bob::io::base::HDF5File& config) {
  load(config);
}

bob::learn::mlp::Machine::~Machine() { }

bob::learn::mlp::Machine& bob::learn::mlp::Machine::operator= (const bob::learn::mlp::Machine& other) {
  if (this != &other)
  {
    m_input_sub.reference(bob::core::array::ccopy(other.m_input_sub));
    m_input_div.reference(bob::core::array::ccopy(other.m_input_div));
    m_weight.resize(other.m_weight.size());
    m_bias.resize(other.m_bias.size());
    m_hidden_activation = other.m_hidden_activation;
    m_output_activation = other.m_output_activation;
    m_buffer.resize(other.m_buffer.size());
    for (size_t i=0; i<other.m_weight.size(); ++i) {
      m_weight[i].reference(bob::core::array::ccopy(other.m_weight[i]));
      m_bias[i].reference(bob::core::array::ccopy(other.m_bias[i]));
      m_buffer[i].reference(bob::core::array::ccopy(other.m_buffer[i]));
    }
  }
  return *this;
}

bool bob::learn::mlp::Machine::operator== (const bob::learn::mlp::Machine& other) const {
  return (bob::core::array::isEqual(m_input_sub, other.m_input_sub) &&
          bob::core::array::isEqual(m_input_div, other.m_input_div) &&
          bob::core::array::isEqual(m_weight, other.m_weight) &&
          bob::core::array::isEqual(m_bias, other.m_bias) &&
          m_hidden_activation->str() == other.m_hidden_activation->str() &&
          m_output_activation->str() == other.m_output_activation->str());
}

bool bob::learn::mlp::Machine::operator!= (const bob::learn::mlp::Machine& other) const {
  return !(this->operator==(other));
}

bool bob::learn::mlp::Machine::is_similar_to(const bob::learn::mlp::Machine& other,
    const double r_epsilon, const double a_epsilon) const
{
  return (bob::core::array::isClose(m_input_sub, other.m_input_sub, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_input_div, other.m_input_div, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_weight, other.m_weight, r_epsilon, a_epsilon) &&
          bob::core::array::isClose(m_bias, other.m_bias, r_epsilon, a_epsilon) &&
          m_hidden_activation->str() == other.m_hidden_activation->str() &&
          m_output_activation->str() == other.m_output_activation->str());
}

void bob::learn::mlp::Machine::load (bob::io::base::HDF5File& config) {
  uint8_t nhidden = config.read<uint8_t>("nhidden");
  m_weight.resize(nhidden+1);
  m_bias.resize(nhidden+1);
  m_buffer.resize(nhidden+1);

  //configures the input
  m_input_sub.reference(config.readArray<double,1>("input_sub"));
  m_input_div.reference(config.readArray<double,1>("input_div"));

  boost::format weight("weight_%d");
  boost::format bias("bias_%d");
  ++nhidden;
  for (size_t i=0; i<nhidden; ++i) {
    weight % i;
    m_weight[i].reference(config.readArray<double,2>(weight.str()));
    bias % i;
    m_bias[i].reference(config.readArray<double,1>(bias.str()));
  }

  //switch between different versions - support for version 2
  if (config.hasAttribute(".", "version")) { //new version
    config.cd("hidden_activation");
    m_hidden_activation = bob::learn::activation::load_activation(config);
    config.cd("../output_activation");
    m_output_activation = bob::learn::activation::load_activation(config);
    config.cd("..");
  }
  else { //old version
    uint32_t act = config.read<uint32_t>("activation");
    m_hidden_activation = bob::learn::activation::make_deprecated_activation(act);
    m_output_activation = m_hidden_activation;
  }

  //setup buffers: first, input
  m_buffer[0].reference(blitz::Array<double,1>(m_input_sub.shape()));
  for (size_t i=1; i<m_weight.size(); ++i) {
    //buffers have to be sized the same as the input for the next layer
    m_buffer[i].reference(blitz::Array<double,1>(m_weight[i].extent(0)));
  }
}

void bob::learn::mlp::Machine::save (bob::io::base::HDF5File& config) const {
  config.setAttribute(".", "version", 1);
  config.setArray("input_sub", m_input_sub);
  config.setArray("input_div", m_input_div);
  config.set("nhidden", (uint8_t)(m_weight.size()-1));
  boost::format weight("weight_%d");
  boost::format bias("bias_%d");
  for (size_t i=0; i<m_weight.size(); ++i) {
    weight % i;
    bias % i;
    config.setArray(weight.str(), m_weight[i]);
    config.setArray(bias.str(), m_bias[i]);
  }
  config.createGroup("hidden_activation");
  config.cd("hidden_activation");
  m_hidden_activation->save(config);
  config.cd("..");
  config.createGroup("output_activation");
  config.cd("output_activation");
  m_output_activation->save(config);
  config.cd("..");
}

void bob::learn::mlp::Machine::forward_ (const blitz::Array<double,1>& input,
    blitz::Array<double,1>& output) {

  //doesn't check input, just computes
  m_buffer[0] = (input - m_input_sub) / m_input_div;

  //input -> hidden[0]; hidden[0] -> hidden[1], ..., hidden[N-2] -> hidden[N-1]
  for (size_t j=1; j<m_weight.size(); ++j) {
    bob::math::prod_(m_buffer[j-1], m_weight[j-1], m_buffer[j]);
    m_buffer[j] += m_bias[j-1];
    for (int i=0; i<m_buffer[j].extent(0); ++i) {
      m_buffer[j](i) = m_hidden_activation->f(m_buffer[j](i));
    }
  }

  //hidden[N-1] -> output
  bob::math::prod_(m_buffer.back(), m_weight.back(), output);
  output += m_bias.back();
  for (int i=0; i<output.extent(0); ++i) {
    output(i) = m_output_activation->f(output(i));
  }
}

void bob::learn::mlp::Machine::forward (const blitz::Array<double,1>& input,
    blitz::Array<double,1>& output) {

  //checks input
  if (m_weight.front().extent(0) != input.extent(0)) {//checks input
    boost::format m("mismatch on the input dimension: expected a vector with %d positions, but you input %d");
    m % m_weight.front().extent(0) % input.extent(0);
    throw std::runtime_error(m.str());
  }
  if (m_weight.back().extent(1) != output.extent(0)) {//checks output
    boost::format m("mismatch on the output dimension: expected a vector with %d positions, but you input %d");
    m % m_weight.back().extent(1) % output.extent(0);
    throw std::runtime_error(m.str());
  }
  forward_(input, output);
}

void bob::learn::mlp::Machine::forward_ (const blitz::Array<double,2>& input,
    blitz::Array<double,2>& output) {

  blitz::Range all = blitz::Range::all();
  for (int i=0; i<input.extent(0); ++i) {
    blitz::Array<double,1> inref(input(i,all));
    blitz::Array<double,1> outref(output(i,all));
    forward_(inref, outref);
  }
}

void bob::learn::mlp::Machine::forward (const blitz::Array<double,2>& input,
    blitz::Array<double,2>& output) {

  //checks input
  if (m_weight.front().extent(0) != input.extent(1)) {//checks input
    boost::format m("mismatch on the input dimension: expected a vector with %d positions, but you input %d");
    m % m_weight.front().extent(0) % input.extent(1);
    throw std::runtime_error(m.str());
  }
  if (m_weight.back().extent(1) != output.extent(1)) {//checks output
    boost::format m("mismatch on the output dimension: expected a vector with %d positions, but you input %d");
    m % m_weight.back().extent(1) % output.extent(1);
    throw std::runtime_error(m.str());
  }
  //checks output
  bob::core::array::assertSameDimensionLength(input.extent(0), output.extent(0));
  forward_(input, output);
}

void bob::learn::mlp::Machine::resize (size_t input, size_t output) {
  m_input_sub.resize(input);
  m_input_sub = 0;
  m_input_div.resize(input);
  m_input_div = 1;
  m_weight.resize(1);
  m_weight[0].reference(blitz::Array<double,2>(input, output));
  m_bias.resize(1);
  m_bias[0].reference(blitz::Array<double,1>(output));
  m_buffer.resize(1);
  m_buffer[0].reference(blitz::Array<double,1>(input));
  setWeights(0);
  setBiases(0);
}

void bob::learn::mlp::Machine::resize (size_t input, size_t hidden, size_t output) {
  std::vector<size_t> vhidden(1, hidden);
  resize(input, vhidden, output);
}

void bob::learn::mlp::Machine::resize (size_t input, const std::vector<size_t>& hidden,
    size_t output) {

  if (hidden.size() == 0) {
    resize(input, output);
    return;
  }

  m_input_sub.resize(input);
  m_input_sub = 0;
  m_input_div.resize(input);
  m_input_div = 1;
  m_weight.resize(hidden.size()+1);
  m_bias.resize(hidden.size()+1);
  m_buffer.resize(hidden.size()+1);

  //initializes first layer
  m_weight[0].reference(blitz::Array<double,2>(input, hidden[0]));
  m_bias[0].reference(blitz::Array<double,1>(hidden[0]));
  m_buffer[0].reference(blitz::Array<double,1>(input));

  //initializes hidden layers
  const size_t NH1 = hidden.size()-1;
  for (size_t i=0; i<NH1; ++i) {
    m_weight[i+1].reference(blitz::Array<double,2>(hidden[i], hidden[i+1]));
    m_bias[i+1].reference(blitz::Array<double,1>(hidden[i+1]));
    m_buffer[i+1].reference(blitz::Array<double,1>(hidden[i]));
  }

  //initializes the last layer
  m_weight.back().reference(blitz::Array<double,2>(hidden.back(), output));
  m_bias.back().reference(blitz::Array<double,1>(output));
  m_buffer.back().reference(blitz::Array<double,1>(hidden.back()));

  setWeights(0);
  setBiases(0);
}

void bob::learn::mlp::Machine::resize (const std::vector<size_t>& shape) {

  if (shape.size() < 2) {
    boost::format m("invalid shape for MLP: %d");
    m % shape.size();
    throw std::runtime_error(m.str());
  }

  if (shape.size() == 2) {
    resize(shape[0], shape[1]);
    return;
  }

  //falls back to the normal case
  size_t input = shape.front();
  size_t output = shape.back();
  std::vector<size_t> vhidden(shape.size()-2);
  for (size_t i=1; i<(shape.size()-1); ++i) vhidden[i-1] = shape[i];
  resize(input, vhidden, output);
}

void bob::learn::mlp::Machine::setInputSubtraction(const blitz::Array<double,1>& v) {
  if (m_weight.front().extent(0) != v.extent(0)) {
    boost::format m("mismatch on the input subtraction dimension: expected a vector with %d positions, but you input %d");
    m % m_weight.front().extent(0) % v.extent(0);
    throw std::runtime_error(m.str());
  }
  m_input_sub.reference(bob::core::array::ccopy(v));
}

void bob::learn::mlp::Machine::setInputDivision(const blitz::Array<double,1>& v) {
  if (m_weight.front().extent(0) != v.extent(0)) {
    boost::format m("mismatch on the input division dimension: expected a vector with %d positions, but you input %d");
    m % m_weight.front().extent(0) % v.extent(0);
    throw std::runtime_error(m.str());
  }
  m_input_div.reference(bob::core::array::ccopy(v));
}

void bob::learn::mlp::Machine::setWeights(const std::vector<blitz::Array<double,2> >& weight) {
  if (m_weight.size() != weight.size()) {
    boost::format m("mismatch on the number of weight layers to set: expected %d layers, but you input %d");
    m % m_weight.size() % weight.size();
  }
  for (size_t i=0; i<m_weight.size(); ++i) {
    if (!bob::core::array::hasSameShape(m_weight[i], weight[i])) {
      boost::format m("mismatch on the shape of weight layer %d");
      m % i;
      throw std::runtime_error(m.str());
    }
  }
  //if you got to this point, the sizes are correct, just set
  for (size_t i=0; i<m_weight.size(); ++i) m_weight[i] = weight[i];
}

void bob::learn::mlp::Machine::setWeights(double v) {
  for (size_t i=0; i<m_weight.size(); ++i) m_weight[i] = v;
}

void bob::learn::mlp::Machine::setBiases(const std::vector<blitz::Array<double,1> >& bias) {
  if (m_bias.size() != bias.size()) {
    boost::format m("mismatch on the number of bias layers to set: expected %d layers, but you input %d");
    m % m_bias.size() % bias.size();
    throw std::runtime_error(m.str());
  }
  for (size_t i=0; i<m_bias.size(); ++i) {
    if (!bob::core::array::hasSameShape(m_bias[i], bias[i])) {
      boost::format m("mismatch on the shape of bias layer %d: expected a vector with length %d, but you input %d");
      m % i % m_bias[i].shape()[0] % bias[i].shape()[0];
      throw std::runtime_error(m.str());
    }
  }
  //if you got to this point, the sizes are correct, just set
  for (size_t i=0; i<m_bias.size(); ++i) m_bias[i] = bias[i];
}

void bob::learn::mlp::Machine::setBiases(double v) {
  for (size_t i=0; i<m_bias.size(); ++i) m_bias[i] = v;
}

void bob::learn::mlp::Machine::randomize(boost::mt19937& rng, double lower_bound, double upper_bound) {
  boost::uniform_real<double> draw(lower_bound, upper_bound);

  for (size_t k=0; k<m_weight.size(); ++k) {
    for (int i=0; i<m_weight[k].extent(0); ++i) {
      for (int j=0; j<m_weight[k].extent(1); ++j) {
        m_weight[k](i,j) = draw(rng);
      }
    }
    for (int i=0; i<m_bias[k].extent(0); ++i) m_bias[k](i) = draw(rng);
  }
}

void bob::learn::mlp::Machine::randomize(double lower_bound, double upper_bound) {
  struct timeval tv;
  gettimeofday(&tv, 0);
  boost::mt19937 rng(tv.tv_sec + tv.tv_usec);
  randomize(rng, lower_bound, upper_bound);
}