/*
 This program is free software; you can redistribute it and/or modify it under
 the terms of the European Union Public Licence - EUPL v.1.1 as published by
 the European Commission.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE. See the European Union Public Licence - EUPL v.1.1
 for more details.

 You should have received a copy of the European Union Public Licence - EUPL v.1.1
 along with this program.

 Further information about the European Union Public Licence - EUPL v.1.1 can
 also be found on the world wide web at http://ec.europa.eu/idabc/eupl

*/

/*
 ------ Copyright (C) 2010 STA Steering Board (space.trajectory.analysis AT gmail.com) ----
*/
/*
------------------ Author: Valentino Zuccarelli  -------------------------------------------------
 ------------------ E-mail: (Valentino.Zuccarelli@gmail.com) ----------------------------
 */

#include "math.h"
#include "Astro-Core/propagateTHREEbody.h"
#include "Astro-Core/threebodyParametersComputation.h"
#include "halorbitcomputation.h"
#include "trajectoryprinting.h"
#include "Astro-Core/EODE/eode.h"
#include <QDebug>
#include <QString>
#include <ctime>
#include <QFile>
#include <QTextStream>
#include <Eigen/LU>
#include <Eigen/QR>

using namespace Eigen;
extern double Grav_Param, bodies_distance;
int halo_num_positions;
#define Pi 3.14159265358979
QString halo_input_data;

void HaloOrbit::lpointsComputation ()
{
    double ni, csi_1, csi_2;

    ni=pow(Grav_Param/(3*(1-Grav_Param)), 1.0/3.0);
    csi_1=ni*(1-ni/3.0-(pow(ni,2.0))/9.0-23*(pow(ni,3.0))/81.0+151*(pow(ni,4.0))/243.0-(pow(ni,5.0))/9.0);
    L1_xposition=(1-Grav_Param-csi_1);		//L1 coordinate
    csi_2=ni*(1+ni/3.0-(pow(ni,2.0))/9.0-31*(pow(ni,3.0))/81.0-119*(pow(ni,4.0))/243.0-(pow(ni,5.0))/9.0);
    L2_xposition=(1+csi_2-Grav_Param);	//L2 coordinate
}

void HaloOrbit::getAnalyticalApprox ()

{
        double gammaL, c[4], r1; int i;
        r1=trajectory.Lpoint_Xposition/bodies_distance;
        ////qdebug()<<bodies_distance;

        if (trajectory.Lpoint_selected==1)
                {
                double oi=1.0-Grav_Param-r1;
                gammaL=fabs(oi);
                for (i=2;i<5;i++)
                        c[i]=(pow(1.0,i)*Grav_Param+pow(-1.0,i)*(1-Grav_Param)*pow(gammaL,i+1)/pow(1.0-gammaL,i+1))/pow(gammaL,3);
                }
        else if (trajectory.Lpoint_selected==2)
                {
                gammaL=fabs(1-Grav_Param-r1);
                for (i=2;i<5;i++)
                        c[i]=(pow(-1.0,i)*Grav_Param+pow(-1.0,i)*(1-Grav_Param)*pow(gammaL,i+1)/pow(1.0+gammaL,i+1))/pow(gammaL,3);
                }
        else if (trajectory.Lpoint_selected==3)
                {
                gammaL=fabs(Grav_Param+r1);
                for (i=2;i<5;i++)
                        c[i]=(1-Grav_Param+(Grav_Param)*pow(gammaL,i+1)/pow(1.0+gammaL,i+1))/pow(gammaL,3);
                }

        double b, cc, lambda, k, delta, d1, d2;

        b=c[2]-2; cc=-(c[2]-1)*(c[2]*2+1);
        lambda=sqrt((-b+sqrt(pow(b,2.0)-4*cc))/2);
        k=(pow(lambda,2)+1+2*c[2])/(2*lambda);
        delta=pow(lambda,2)-c[2];
        d1=3*pow(lambda,2)*(k*(6*pow(lambda,2.0)-1)-2*lambda)/k;
        d2=8*pow(lambda,2)*(k*(11*pow(lambda,2.0)-1)-2*lambda)/k;

        double a21, a22, a23, a24, a31, a32, b21, b22, b31, b32, d21, d31, d32;

        a21=3*c[3]*(pow(k,2.0)-2)/(4*(1+2*c[2]));
        a22=3*c[3]/(4*(1+2*c[2]));
        a23=-3*c[3]*lambda*(3*lambda*pow(k,3.0)-6*k*(k-lambda)+4)/(4*k*d1);
        a24=-3*c[3]*lambda*(3*lambda*k+2)/(4*k*d1);

        b21=-3*c[3]*lambda*(3*k*lambda-4)/(2*d1);
        b22=3*c[3]*lambda/d1;
        a31=-9*lambda*(4*c[3]*(k*a23-b21)+k*c[4]*(4+pow(k,2)))/(4*d2)+((9*pow(lambda,2)+1-c[2])/(2*d2))*(3*c[3]*(2*a23-k*b21)+c[4]*(2+3*pow(k,2)));
        d21=-c[3]/(2*pow(lambda,2));
        a32=-(9*lambda*(4*c[3]*(k*a24-b22)+k*c[4])/4+3*(9*pow(lambda,2)+1-c[2])*(c[3]*(k*b22+d21-2*a24)-c[4])/2)/d2;
        b31=3*(8*lambda*(3*c[3]*(k*b21-2*a23)-c[4]*(2+3*pow(k,2)))+(9*pow(lambda,2)+1+2*c[2])*(4*c[3]*(k*a23-b21)+k*c[4]*(4+pow(k,2))))/(8*d2);
        b32=(9*lambda*(c[3]*(k*b22+d21-2*a24)-c[4])+3*(9*pow(lambda,2)+1+2*c[2])*(4*c[3]*(k*a24-b22)+k*c[4])/8)/d2;
        d31=3*(4*c[3]*a24+c[4])/(64*pow(lambda,2));
        d32=3*(4*c[3]*(a23-d21)+c[4]*(4+pow(k,2)))/(64*pow(lambda,2));

        double teta, s1, s2, l1, l2, a1, a2;

        a1=-3*c[3]*(2*a21+a23+5*d21)/2-3*c[4]*(12-pow(k,2))/8;
        a2=3*c[3]*(a24-2*a22)/2+9*c[4]/8;
        s1=(3*c[3]*(2*a21*(pow(k,2)-2)-a23*(pow(k,2)+2)-2*k*b21)/2-3*c[4]*(3*pow(k,4)-8*pow(k,2)+8)/8)/(2*lambda*(lambda*(1+pow(k,2))-2*k));
        s2=(3*c[3]*(2*a22*(pow(k,2)-2)+a24*(pow(k,2)+2)+2*k*b22+5*d21)/2+3*c[4]*(12-pow(k,2))/8)/(2*lambda*(lambda*(1+pow(k,2))-2*k));
        l1=a1+2*s1*pow(lambda,2);
        l2=a2+2*s2*pow(lambda,2);

        double Axmin, R1;

        R1=bodies_distance*gammaL;
        Axmin=(sqrt(fabs(delta/l1)))*R1;

        if (trajectory.init_data.Ax_amplitude<Axmin)
                trajectory.init_data.Ax_amplitude=Axmin;
        double Ax, Az;
        Az=trajectory.init_data.Az_amplitude/R1;
        Ax=trajectory.init_data.Ax_amplitude/R1;

        teta=0; i=0;

        double temporary_Zposition=((Az*cos(teta)+d21*Ax*Az*(cos(2*teta)-3)+cos(3*teta)*(d32*Az*pow(Ax,2)-d31*pow(Az,3)))*R1)/bodies_distance;
        if (temporary_Zposition<0)
                i=20;
        teta=Pi*0.05*i;
        double temporary_Xposition=((a21*pow(Ax,2)+a22*pow(Az,2)-Ax*cos(teta)+(a23*pow(Ax,2)-a24*pow(Az,2))*cos(2*teta)+(a31*pow(Ax,3)-a32*Ax*pow(Az,2))*cos(3*teta))*R1)/bodies_distance;

        trajectory.init_data.State.position.x()=r1+temporary_Xposition;
        trajectory.init_data.State.position.z()=((Az*cos(teta)+d21*Ax*Az*(cos(2*teta)-3)+cos(3*teta)*(d32*Az*pow(Ax,2)-d31*pow(Az,3)))*R1)/bodies_distance;
        trajectory.init_data.State.velocity.y()=((k*Ax*cos(teta)*lambda+(b21*pow(Ax,2)-b22*pow(Az,2))*cos(teta*2)*2*lambda+(b31*pow(Ax,3)-b32*Ax*pow(Az,2))*cos(3*teta)*3*lambda))*R1/bodies_distance;
}

MatrixXd STM(6,6);

void HaloOrbit::getNumericalApprox (int system, int &MAX_integration_time)//, const Eigen::MatrixXd STM)

{
    clock_t start, end;
    start= clock();
    time_t t0, t1, diff;
    double time_elapsed;

    double paramv[1]={0}; double deriv[42]; double state[42];
    trajectory.init_data.State.velocity.x()=trajectory.init_data.State.velocity.z()=trajectory.init_data.State.position.y()=trajectory.init_data.period=0;
    trajectory.final_data.State.velocity.x()=trajectory.final_data.State.velocity.z()=1;

    double timein, timefin, initialtimefin, relerr, abserr, accuracy, count, count2, T, step; int flag, k, q, p, l=0;
    relerr=abserr=accuracy=1E-9; timefin=0; initialtimefin=0.25; //period   // 0.25!!!!!!!!!!!!!!!!!!!!!!! per velocizzare 1.0

    double correction[6]; double z_correction, x_correction; double deriv_vel[3];

    if (trajectory.init_data.State.velocity.y()<0) //check the initial y-velocity sign
    {l=1;}

    while (fabs(trajectory.final_data.State.velocity.x())>10e-8 || fabs( trajectory.final_data.State.velocity.z())>10e-8) //stop condition
    {
        T=0; step=0.01;
        ////qdebug()<<"before change"<<timefin;
        //if (timefin>initialtimefin)
            //timefin=timefin-0.8;
        //else
//            timefin=initialtimefin;
        timefin=0.3;
        count=1;

       while (count>0)
       {
           flag=1; timein=0; timefin=timefin+step;
           for (p=0; p<42; p++)
           {
               state[p]=0;
               deriv[p]=0;
           }
               state[1]=trajectory.init_data.State.velocity.y(); state[5]=trajectory.init_data.State.position.z();
               state[0]=trajectory.init_data.State.velocity.x(); state[2]=trajectory.init_data.State.velocity.z();
               state[3]=trajectory.init_data.State.position.x(); state[4]=trajectory.init_data.State.position.y();

               state[41]=state[6]=state[13]=state[20]=state[27]=state[34]=1;
               //qDebug()<<"step0"<<state[0]<<state[1]<<state[2]<<state[3]<<state[4]<<state[5];

               while (flag!=2)
               {
                   flag=Runge_Kutta_Fehlberg(halorbit_EOM, 42, state, paramv, deriv, &timein, timefin, &relerr, abserr, flag);
                   if (flag==4)
                       flag=Runge_Kutta_Fehlberg(halorbit_EOM, 42, state, paramv, deriv, &timein, timefin, &relerr, abserr, flag);
               }
               //qDebug()<<"step1"<<state[0]<<state[1]<<state[2]<<state[3]<<state[4]<<state[5];
               count=state[4];
               if (l==1)
                   count=-state[4];
       }

       count2=1;
       end= clock(); time_elapsed=(double(end-start))/CLOCKS_PER_SEC;

       if (time_elapsed>MAX_integration_time)
       {
           ////qdebug()<<"Max time allowed:"<<MAX_integration_time<<"time elapsed"<<time_elapsed;
           trajectory.error=1;
           return;
       }

       while (fabs(count2)>10e-12)
       {
           timefin=timefin-step;
           step=step/10.0;
           count2=1;
           while (count2>0)
           {
               for (p=0; p<42; p++)
               {
                   state[p]=0;
                   deriv[p]=0;
               }
                   flag=1; timein=0; timefin=timefin+step;
                   state[1]=trajectory.init_data.State.velocity.y(); state[5]=trajectory.init_data.State.position.z();
                   state[0]=trajectory.init_data.State.velocity.x(); state[2]=trajectory.init_data.State.velocity.z();
                   state[3]=trajectory.init_data.State.position.x(); state[4]=trajectory.init_data.State.position.y();
                   state[41]=state[6]=state[13]=state[20]=state[27]=state[34]=1;
                   //qDebug()<<"step2"<<state[0]<<state[1]<<state[2]<<state[3]<<state[4]<<state[5];

                   while (flag!=2)
                   {
                       flag=Runge_Kutta_Fehlberg(halorbit_EOM, 42, state, paramv, deriv, &timein, timefin, &relerr, abserr, flag);
                       if (flag==4)
                           flag=Runge_Kutta_Fehlberg(halorbit_EOM, 42, state, paramv, deriv, &timein, timefin, &relerr, abserr, flag);
                   }
                   //qDebug()<<"step3"<<state[0]<<state[1]<<state[2]<<state[3]<<state[4]<<state[5];

                   count2=state[4];
                   if (l==1)
                       count2=-state[4];

                   end=clock(); time_elapsed=(double(end-start))/CLOCKS_PER_SEC;

                   if (time_elapsed>MAX_integration_time)
                   {
                       ////qdebug()<<"Max time allowed:"<<MAX_integration_time<<"time elapsed"<<time_elapsed;
                       trajectory.error=1;
                       return;
                   }
            }

           trajectory.final_data.State.velocity.x()=state[0]; trajectory.final_data.State.velocity.y()=state[1]; trajectory.final_data.State.velocity.z()=state[2];
           trajectory.final_data.State.position.x()=state[3]; trajectory.final_data.State.position.y()=state[4]; trajectory.final_data.State.position.z()=state[5];
           trajectory.final_data.period=T=2*timefin;
           correction[0]=state[12]; correction[1]=state[14]; correction[2]=state[24]; correction[3]=state[26]; correction[4]=state[36]; correction[5]=state[38];
       }

       ////qdebug()<<"initial state"<<trajectory.init_data.State.position.x()<<trajectory.init_data.State.position.z()<<trajectory.init_data.State.velocity.y();
       ////qdebug()<<"final state"<<trajectory.final_data.State.position.y()<<trajectory.final_data.State.velocity.x()<<trajectory.final_data.State.velocity.z()<<timefin;
       double C;
       Jacobi (Grav_Param, trajectory.final_data.State.position.x(), trajectory.final_data.State.position.y(), trajectory.final_data.State.position.z(), trajectory.final_data.State.velocity.x(), trajectory.final_data.State.velocity.y(), trajectory.final_data.State.velocity.z(),C);
       ////qdebug()<<"C"<<C;
       if (fabs(trajectory.final_data.State.velocity.x())<10e-8 && fabs(trajectory.final_data.State.velocity.z())<10e-8)
       {
           for (k=0;k<6;k++)
           {
               for (p=0;p<6;p++)
               {
                   STM(k,p)=state[6*(k+1)+p];
               }
           }
       }
       else
       {
           double r1, r2;
           r1=sqrt(pow(trajectory.final_data.State.position.x()+Grav_Param,2)+pow(trajectory.final_data.State.position.y(),2)+pow(trajectory.final_data.State.position.z(),2));
           r2=sqrt(pow(trajectory.final_data.State.position.x()-1+Grav_Param,2)+pow(trajectory.final_data.State.position.y(),2)+pow(trajectory.final_data.State.position.z(),2));
           deriv_vel[0]=2*trajectory.final_data.State.velocity.y()+trajectory.final_data.State.position.x()-(1-Grav_Param)*(trajectory.final_data.State.position.x()+Grav_Param)/(pow(r1,3))+ Grav_Param*(1-Grav_Param-trajectory.final_data.State.position.x())/(pow(r2,3));
           deriv_vel[2]=-(1-Grav_Param)*trajectory.final_data.State.position.z()/(pow(r1,3))-Grav_Param*trajectory.final_data.State.position.z()/(pow(r2,3));

           MatrixXd correction_matrix(2,2);
           for (k=0; k<4; k++)
           {
               q=(k+2)/2;
               int col = (k+1)/2-(q-1);
               correction_matrix(q-1,col)=correction[k+2]-deriv_vel[2*(q-1)]*correction[k-2*(q-1)]/trajectory.final_data.State.velocity.y();
               //correction_matrix[k]=correction[k+2]-deriv_vel[2*(q-1)]*correction[k-2*(q-1)]/trajectory.final_data.State.velocity.y();
           }
           //correction_matrix.inverse();
           ////qdebug()<<"inverse!"<<correction_matrix.inverse()(0,0)<<correction_matrix.inverse()(0,1)<<correction_matrix.inverse()(1,0)<<correction_matrix.inverse()(1,1);
           //determinant= correction_matrix[0]*correction_matrix[3]-correction_matrix[2]*correction_matrix[1];
           //inverse_correction_matrix[0]=correction_matrix[3]/determinant;
           //inverse_correction_matrix[1]=-correction_matrix[1]/determinant;
           //inverse_correction_matrix[2]=-correction_matrix[2]/determinant;
           //inverse_correction_matrix[3]=correction_matrix[0]/determinant;

               //x_correction=-inverse_correction_matrix[0]*trajectory.final_data.State.velocity.x()-inverse_correction_matrix[1]*trajectory.final_data.State.velocity.z();
               //z_correction=-inverse_correction_matrix[2]*trajectory.final_data.State.velocity.x()-inverse_correction_matrix[3]*trajectory.final_data.State.velocity.z();
           x_correction=-correction_matrix.inverse()(0,0)*trajectory.final_data.State.velocity.x()-correction_matrix.inverse()(0,1)*trajectory.final_data.State.velocity.z();
           z_correction=-correction_matrix.inverse()(1,0)*trajectory.final_data.State.velocity.x()-correction_matrix.inverse()(1,1)*trajectory.final_data.State.velocity.z();
               trajectory.init_data.State.position.x()=trajectory.init_data.State.position.x()+x_correction;
               trajectory.init_data.State.position.z()=trajectory.init_data.State.position.z()+z_correction;
       }

       }

//qdebug()<<"here|";
        int trajectory_error=0;
        state[6]=T; state[0]=trajectory.init_data.State.velocity.x(); state[1]=trajectory.init_data.State.velocity.y(); state[2]=trajectory.init_data.State.velocity.z();
        state[3]=trajectory.init_data.State.position.x(); state[4]=trajectory.init_data.State.position.y(); state[5]=trajectory.init_data.State.position.z();
        QString save_path = "3BMhalorbit";
        halo_input_data=save_path=save_path+QString::number(trajectory.init_data.Ax_amplitude,'f',0)+QString::number(trajectory.init_data.Az_amplitude,'f',0)+".stae";
        halo_num_positions=trajectory.Num_positions;
        trajectory_integration(state, trajectory.Num_positions, save_path, trajectory_error);
        ////qdebug() << "halo file: " << save_path;

        if (trajectory_error==2) //if the halo trajectory.txt has been not opened a message error must be given
        {
            trajectory.error=trajectory_error;
            return;
        }

        //file 3BMsystem.stam
        QString file = "3BMsystem.stam";
        QFile threeBMsystemFile(file);
        threeBMsystemFile.open(QIODevice::WriteOnly | QIODevice::Text);

        QTextStream outsys(&threeBMsystemFile);
        outsys.setRealNumberNotation(QTextStream::FixedNotation);
        outsys.setRealNumberPrecision(16);
        if (system==12)
            system=30; //user system, as ICD states

        outsys<<system<<"\n";
        outsys<<Grav_Param<<"\n";
        outsys<<bodies_distance<<"\n";
        outsys<<L1_xposition<<"\n";
        outsys<<L2_xposition<<"\n";
        threeBMsystemFile.close();
        ////qdebug()<<file;

        //computation of the real amplitudes obtained for the halo orbit
        QFile haloFile1(save_path);
        if (!haloFile1.open(QIODevice::ReadOnly | QIODevice::Text))
            {trajectory.error=3;
                return;
            }

        QTextStream in(&haloFile1);
        double t,Vx,Vy,Vz,x,y,z,X_max=0, X_min=10,Z_max=0, Z_min=10;
        for (p=0;p<trajectory.Num_positions;p++) //search for the maximum and minimum values of x and z coordinates
                {
                in>>t>>x>>y>>z>>Vx>>Vy>>Vz;
                //qDebug()<<x<<z;
                if (x<X_min)
                        X_min=x;
                if (x>X_max)
                        X_max=x;
                if (z<Z_min)
                        Z_min=z;
                if (z>Z_max)
                        Z_max=z;
                }
        haloFile1.close();

        trajectory.final_data.Ax_amplitude=(fabs(X_max)-fabs(X_min))/2;	trajectory.final_data.Az_amplitude=(fabs(Z_max)+fabs(Z_min))/2; //computation of the amplitudes obtained
        if (trajectory.init_data.Az_amplitude<0)
            trajectory.final_data.Az_amplitude=-trajectory.final_data.Az_amplitude;
        //qDebug()<<trajectory.final_data.Az_amplitude<<trajectory.final_data.Ax_amplitude<<bodies_distance;

        //check on the real amplitudes obtained, if too different the manifolds computation will be not allowed
        if (fabs((trajectory.final_data.Az_amplitude*bodies_distance-trajectory.init_data.Az_amplitude)/trajectory.init_data.Az_amplitude)>15 || fabs((trajectory.final_data.Ax_amplitude*bodies_distance-trajectory.init_data.Ax_amplitude)/trajectory.init_data.Ax_amplitude)>15)
            trajectory.error=4;
}

void HaloOrbit::getManifolds (double eigen, int points_number, double period_manifolds, int &first)

{
        double initial_condition_perturbed[7], initial_condition_0[7]; const char * manifolds_save_path;
        initial_condition_perturbed[6]=period_manifolds;

        QFile file_manifolds_settings ("3BMmanifolds_settings.stam");
        if (first==1)
                {if (!file_manifolds_settings.open(QIODevice::WriteOnly | QIODevice::Text))
                        {manifolds.error=3;
                        return;}
                }
        else
                {if (!file_manifolds_settings.open(QIODevice::Append | QIODevice::Text))
                        {manifolds.error=3;
                        return;}
                }

        QTextStream settings(&file_manifolds_settings);

        if (manifolds.kind==1)	//information about the manifolds required
                manifolds_save_path="3BMmanifold_rightStable.stae";
        else if (manifolds.kind==2)
                manifolds_save_path="3BMmanifold_rightUnstable.stae";
        else if (manifolds.kind==3)
                manifolds_save_path="3BMmanifold_leftStable.stae";
        else if (manifolds.kind==4)
                manifolds_save_path="3BMmanifold_leftUnstable.stae";
        else
                {manifolds.error=5;
                return;
                }
        settings<<manifolds.kind<<"\n"<<trajectory.Num_positions<<"\n";	//the kind of manifold and the number of orbits are printed

        if (period_manifolds!=0)
                settings<<points_number<<"\n";	//the number of positions per orbit is printed in case a time of integration has been specified
        file_manifolds_settings.close();

        int i;
       /////// if(trajectory.Num_positions>0) //normal manifolds computation

             QString halo_data = "3BMhalorbit";
        halo_data=halo_data+QString::number(trajectory.init_data.Ax_amplitude,'f',0)+QString::number(trajectory.init_data.Az_amplitude,'f',0)+".stae";
        QFile file_halo2(halo_data);
        if (!file_halo2.open(QIODevice::ReadOnly | QIODevice::Text))
        {
            manifolds.error=1;
            return;
        }
        QTextStream in(&file_halo2);
        for (i=1;i<trajectory.Num_positions+1;i++)
                {
                in>>initial_condition_0[6]>>initial_condition_0[3]>>initial_condition_0[4]>>initial_condition_0[5]>>initial_condition_0[0]>>initial_condition_0[1]>>initial_condition_0[2];

                for (int j=0;j<6;j++)
                        {initial_condition_perturbed[j]=initial_condition_0[j]-manifolds.epsilon*eigen*initial_condition_0[j];}
                if (eigen<1) //condition in case of stable manifolds
                {
                    initial_condition_perturbed[0]=-initial_condition_perturbed[0];
                    initial_condition_perturbed[4]=-initial_condition_perturbed[4];
                    initial_condition_perturbed[5]=-initial_condition_perturbed[5];
                    i=-i;
                }
                ////qdebug()<<"Debug| Bon"<<initial_condition_perturbed[0]<<initial_condition_perturbed[1]<<initial_condition_perturbed[2]<<initial_condition_perturbed[3]<<initial_condition_perturbed[4]<<initial_condition_perturbed[5]<<initial_condition_perturbed[6]<<points_number<<manifolds_save_path<<manifolds.error<<i<<trajectory.Lpoint_Xposition<<manifolds.kind;
                manifolds_integration(initial_condition_perturbed, points_number, manifolds_save_path, manifolds.error, i, trajectory.Lpoint_Xposition, manifolds.kind);
                i=abs(i);
                if (manifolds.error!=0)
                    return;
                }

                file_halo2.close();
                if (i!=trajectory.Num_positions+1)
                manifolds.error=5;

        /* manifolds computation in case of transfer optimization
           in this case only on trajectory is randomly computed
        */

       /*
 else

        {
            double paramv[1]={0}; double deriv[5]; double timefin, timein=0;
            double relerr, abserr, accuracy; abserr=relerr=accuracy=1E-9;

            for (int p=0; p<6; p++)
                deriv[p]=0;
            int flag=1;

            extern double fractionOrbit;
            timefin=trajectory.final_data.period*fractionOrbit;
            //qdebug()<<"time"<<timefin<<trajectory.final_data.period<<fractionOrbit;
            initial_condition_0[3]=trajectory.final_data.State.position.x();
            initial_condition_0[4]=trajectory.final_data.State.position.y();
            initial_condition_0[5]=trajectory.final_data.State.position.z();
            initial_condition_0[0]=trajectory.final_data.State.velocity.x();
            initial_condition_0[1]=trajectory.final_data.State.velocity.y();
            initial_condition_0[2]=trajectory.final_data.State.velocity.z();
//qdebug()<<"Debug| 1"<<initial_condition_0[0]<<initial_condition_0[1]<<initial_condition_0[2]<<initial_condition_0[3]<<initial_condition_0[4]<<initial_condition_0[5];
//qdebug()<<timein;
double initial_condition_1[5];
        while (flag!=2)
        {
            flag=Runge_Kutta_Fehlberg(threebody_EOM, 6, initial_condition_0, paramv, deriv, &timein, timefin, &relerr, abserr, flag );
            if (flag==4)
                flag=Runge_Kutta_Fehlberg(threebody_EOM, 6, initial_condition_0, paramv, deriv, &timein, timefin, &relerr, abserr, flag );
        //qdebug()<<timein;
        }
        for (int u=0; u<6; u++)
            initial_condition_1[u]=initial_condition_0[u];

//qdebug()<<"Debug| 2"<<initial_condition_1[0]<<initial_condition_1[1]<<initial_condition_1[2]<<initial_condition_1[3]<<initial_condition_1[4]<<initial_condition_1[5];
        initial_condition_1[6]=0; //required intersection with a main body
                for (int j=0;j<6;j++)
                        {initial_condition_perturbed[j]=initial_condition_1[j]-manifolds.epsilon*eigen*initial_condition_1[j];}
                i=1;
                //qdebug()<<"Debug| 3"<<initial_condition_perturbed[0]<<initial_condition_perturbed[1]<<initial_condition_perturbed[2]<<initial_condition_perturbed[3]<<initial_condition_perturbed[4]<<initial_condition_perturbed[5];
                if (eigen<1) //condition in case of stable manifolds
                {
                    initial_condition_perturbed[0]=-initial_condition_perturbed[0];
                    initial_condition_perturbed[4]=-initial_condition_perturbed[4];
                    initial_condition_perturbed[5]=-initial_condition_perturbed[5];
                    i=-1;
                }
                //qdebug()<<"Debug| final"<<initial_condition_perturbed[0]<<initial_condition_perturbed[1]<<initial_condition_perturbed[2]<<initial_condition_perturbed[3]<<initial_condition_perturbed[4]<<initial_condition_perturbed[5]<<initial_condition_perturbed[6]<<points_number<<manifolds_save_path<<manifolds.error<<i<<trajectory.Lpoint_Xposition<<manifolds.kind;
                manifolds_integration(initial_condition_perturbed, points_number, manifolds_save_path, manifolds.error, i, trajectory.Lpoint_Xposition, manifolds.kind);
                if (manifolds.error!=0)
                    return;
        }
        */

        //qdebug()<<manifolds_save_path;
        //qdebug()<<"deviation:"<<manifolds.epsilon;

        ////qdebug()<<manifolds_save_path;
        ////qdebug()<<"deviation:"<<manifolds.epsilon;

}

//subroutine to compute Eigenvalues
void HaloOrbit::getEigenvalues ()

{
        MatrixXd mat1(6,6), mat2(6,6), mat3(6,6), fi(6,6);//, STM_inv(6,6);
        mat2.setZero();
        mat3.setZero();
        mat1.setIdentity();
        mat1=-mat1;

        mat2(0,3)=mat2(1,4)=mat2(2,5)=-1;
        mat2(3,0)=mat2(4,1)=mat2(5,2)=1;
        mat2(3,4)=-2; mat2(4,3)=2;

        mat3(0,1)=-2; mat3(1,0)=2;
        mat3(0,3)=mat3(1,4)=mat3(2,5)=1;
        mat3(3,0)=mat3(4,1)=mat3(5,2)=-1;
        fi=mat1*STM.inverse()*mat2*mat3;

        //Eigen libray doesn't support the eigenvalues computation of any matrix, but only of selfAdjoint matrix.
        //because the STM is not such a matrix, the eigenvalues are computed in an alternative way.
        //When the eigenvalues computation will be available also for this kind of matrix, it's stronly recommended
        //to change this subroutine, by using the Eigen library.
        //the following hidden part is the code able to compute eigenvalues for selfAdjoint matrix. By replacing that
        //specific line, it will be possible to use the new subroutine of the Eigen library, when available.
#if 0
        //
        VectorXd eigenvalues_vector(6);
        eigenvalues_vector=fi.marked<SelfAdjoint>().eigenvalues();
        //VectorXd eigenvalues(6);
//cout<<fi;

        //fi.eigenvalues();
        //qdebug()<<"fi";
        //qdebug()<<fi(0,0)<<fi(0,1)<<fi(0,2)<<fi(0,3)<<fi(0,4)<<fi(0,5);
        //qdebug()<<fi(1,0)<<fi(1,1)<<fi(1,2)<<fi(1,3)<<fi(1,4)<<fi(1,5);
        //qdebug()<<fi(2,0)<<fi(2,1)<<fi(2,2)<<fi(2,3)<<fi(2,4)<<fi(2,5);
        //qdebug()<<fi(3,0)<<fi(3,1)<<fi(3,2)<<fi(3,3)<<fi(3,4)<<fi(3,5);
        //qdebug()<<fi(4,0)<<fi(4,1)<<fi(4,2)<<fi(4,3)<<fi(4,4)<<fi(4,5);
        //qdebug()<<fi(5,0)<<fi(5,1)<<fi(5,2)<<fi(5,3)<<fi(5,4)<<fi(5,5);
        //qdebug()<<"eigen";
        //qdebug()<<evve(0)<<evve(1)<<evve(2)<<evve(3)<<evve(4)<<evve(5);
        ////qdebug()<<eigenvalues(0)<<eigenvalues(1)<<eigenvalues(2)<<eigenvalues(3)<<eigenvalues(4)<<eigenvalues(5);

#endif

         MatrixXd mm1(6,6);
         mm1=fi;
         double step=0.001;
         double l=0; double h=1; int i;

         while (abs(h)>0.00001)
         {
             if (l>1)
             {
                 l=0;
                 step=step/10;}
                 l=step+l;
                 for (i=0;i<6;i++)
                 {
                     mm1(i,i)=fi(i,i)-l;}
                     h=mm1.determinant();
                     if (step<0.0000000001)
                     {
                         manifolds.error=4;
                         return;}
                     }

         manifolds.eigen1=l;
         manifolds.eigen2=1/l;
         //double index=0.5*(manifolds.eigen1+1/manifolds.eigen1); //stability index
}


//#include "Aggregate_Gen.hpp"
#include <iostream>
#include <vector>
#include <cmath>
#include <random>
#include <chrono>
#include <Eigen/Dense>
#include <omp.h>
#include "aggregate_gen_SA.hpp"
using namespace Eigen;
using namespace std;

//return pos_sph matrix
vector<MatrixXd> aggregate_gen_CCA(const double& a, const int& num_sph_SA, const int& levels, const double& kf, const double& Df, const double& tol){

  vector<MatrixXd> pos_sph_agg_output; // vector of aggregate geometry from level 0 to "levels".
  int iter1max {1000};
  int iter2max {10000};

  int num_SA_agg= pow(2,levels);
  int final_totnum_sph= num_SA_agg*num_sph_SA;
  cout << "final totnum_sph= " << final_totnum_sph << endl;

  vector<MatrixXd> pos_sph_SAagg;
  for(int i= 0; i<num_SA_agg; ++i ) pos_sph_SAagg.push_back(aggregate_gen_SA(a,num_sph_SA,kf,Df,tol));

  pos_sph_agg_output.push_back(pos_sph_SAagg[0]); // level 0 aggregate

  vector<MatrixXd> pos_sph_agg_old;

  // start time
  chrono::time_point<chrono::steady_clock> CCA_start_time= chrono::steady_clock::now();

  for(int k= 1; k <= levels; ++k){
      cout << "Generating aggregates in " << k << "th level ... please wait ..." << endl;
      vector<MatrixXd> pos_sph_agg;

      for(int L= 1; L <= pow(2,levels-k); ++L){
          //cout << "generating " << L << " th " << "aggregate in level" << k << endl;
          int N1= num_sph_SA*pow(2,k-1);
          int N2= N1;

          // aggregation process in k-th level
          MatrixXd pos_sph_agg1(3,N1);
          MatrixXd pos_sph_agg2(3,N2);

          if(k==1){
            pos_sph_agg1= pos_sph_SAagg[2*L-2];
            pos_sph_agg2= pos_sph_SAagg[2*L-1];
          } else {
            pos_sph_agg1= pos_sph_agg_old[2*L-2];
            pos_sph_agg2= pos_sph_agg_old[2*L-1];
          }

          // random rotation of aggregate 1 around the centroid (origin)
          double theta,phi; // polar angles
          theta= acos(2.0f*urf(re)-1.0f);
          phi= 2*M_PI*urf(re);

          Vector3d nvec; // unit vector of rotation axis
          nvec << sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta); //axis of rotation randomly choosen over unit sphere

          double psi; // angle of rotation
          psi= 2*M_PI*urf(re); // randomly choosen rotation angle

          Matrix3d rot_mat; // 3D rotation matrix
          rot_mat= AngleAxisd(psi,nvec);

          for(int i= 0; i < pos_sph_agg1.cols(); ++i) pos_sph_agg1.col(i)=rot_mat*pos_sph_agg1.col(i); // random rotation of agg1

          double R1_2; // square of gyration radius of agg1
          double R2_2; // square of gyration radius of agg2
          R1_2= a*a + pos_sph_agg1.colwise().squaredNorm().mean();
          R2_2= a*a + pos_sph_agg2.colwise().squaredNorm().mean();

          double rN;
          rN= a*a*(N1+N2)*(N1+N2)/(N1*N2)*pow((N1+N2)/kf,2/Df)-(N1+N2)/N2*R1_2-(N1+N2)/N1*R2_2;
          rN= sqrt(rN);

          bool found {false};

          //--- translation of agg1 to random position on the sphere of radius rN
          for(int iter1= 0; iter1 != iter1max; ++iter1){

              pos_sph_agg1.colwise() -= pos_sph_agg1.rowwise().mean(); // translate the centroid of agg1 to origin

              double phi;
              phi= 2*M_PI*urf(re);
              double u; // uniform random number [-1.0,1.0)
              u= 2*urf(re)-1.0;

              Vector3d cen_pos_agg1; // centroid of agg1
              cen_pos_agg1 << sqrt(1.0-u*u)*cos(phi), sqrt(1.0-u*u)*sin(phi), u;
              cen_pos_agg1 *= rN; // cen_pos_agg1 is set to a uniform random cartesian position (x,y,z) over the spherical surface of radius rN
              pos_sph_agg1.colwise() += cen_pos_agg1;

              double theta;
              theta= acos(2.0*urf(re)-1.0);
              phi= 2*M_PI*urf(re);
              Vector3d nvec; // unit vector of rotation axis
              nvec << sin(theta)*cos(phi),sin(theta)*sin(phi),cos(theta); //axis of rotation randomly choosen over unit sphere

              // random rotation of aggregate 2 around the centroid (origin)
              #pragma omp parallel for
              for(int iter2= 0; iter2 < iter2max; ++iter2){
                  bool found_local;
                  #pragma omp atomic read
                  found_local= found;
                  if(found) continue;

                  MatrixXd pos_sph_agg1_thread(3,N1); // thread local copy of pos_sph_agg1
                  MatrixXd pos_sph_agg2_thread(3,N2); // thread local copy of pos_sph_agg2
                  pos_sph_agg1_thread= pos_sph_agg1;
                  pos_sph_agg2_thread= pos_sph_agg2;

                  double psi; // angle of rotation

                  #pragma omp critical
                  {
                     psi= 2*M_PI*urf(re);
                  }

                  Matrix3d rot_mat; // 3D rotation matrix
                  rot_mat= AngleAxisd(psi,nvec);
                  for(int i= 0; i < N2; ++i) pos_sph_agg2_thread.col(i)=rot_mat*pos_sph_agg2_thread.col(i); // random rotation of agg2
                  //  cout << "iter2 pos_sph_agg2 " << endl << pos_sph_agg2 << endl;

                  bool attached {false};
                  for(int i= 0; i < N2; ++i){
                      RowVectorXd dist_pair(N1);
                      dist_pair=(pos_sph_agg1_thread.colwise()-pos_sph_agg2_thread.col(i)).colwise().norm();
                    //  cout << "dist_pair" << endl << dist_pair << endl;
                      double mindist= dist_pair.minCoeff();
                      //cout << "2*a, mindist " << 2*a << ' ' << mindist << endl;
                      bool overrapped {mindist <= 2*a};
                      if(overrapped) {
                        //exit; // 20180813 removed
                        attached= false; // 20180813 added
                        break; // 20180813 added
                      } else {
                        attached= attached || (mindist < 2*a+tol);
                      }
                  }

                  // construction of new aggregate if attached condition is satisfied
                  if(attached){
                      //cout << "attached " << attached << endl;
                      MatrixXd pos_sph_agg_tmp(3,N1+N2);
                      pos_sph_agg_tmp.block(0,0,3,N1)= pos_sph_agg1_thread;
                      pos_sph_agg_tmp.block(0,N1,3,N2)= pos_sph_agg2_thread;
                      pos_sph_agg_tmp.colwise() -= pos_sph_agg_tmp.rowwise().mean(); // translate the centroid of new agg to origin
                      #pragma omp critical
                      {
                          pos_sph_agg.push_back(pos_sph_agg_tmp); // update the aggregate
                          found = true;
                          //break; // exit iter2 loop
                          if(k==levels)  cout << "number of threads for parallel search: " << omp_get_num_threads() << endl;
                      }

                  }
              }// iter2 loop

              if(found) break; // exit iter1 loop

              if(iter1 == iter1max){
                  cout << "calculation failed: iter1 reachs iter1max! " << endl;
                  abort();
              }
          }// iter1 loop
      }// L loop

      pos_sph_agg_old= move(pos_sph_agg);
      pos_sph_agg_output.push_back(pos_sph_agg_old[0]);

  }// k loop

  auto CCA_end_time= chrono::steady_clock::now();

  cout << "CCA computation time: "
  << chrono::duration_cast<chrono::milliseconds>(CCA_end_time - CCA_start_time).count() << " milliseconds" << endl;

  // geometry check for output
  bool non_overlapped_all {true};
  bool attached_at_least_1pair_all {true};

  for(int k= 0; k <= levels; ++k){
      MatrixXd pos_sph_agg_k_out {pos_sph_agg_output[k]};
      int num_sph = num_sph_SA*pow(2,k);
      assert(num_sph == pos_sph_agg_k_out.cols());
      // final check of the attached condition
      bool non_overlapped;
      ArrayXd min_diff_from2a(num_sph);
      for(int i= 0; i< num_sph; ++i){
          ArrayXd distance_from_this_sph(num_sph);
          distance_from_this_sph= (pos_sph_agg_k_out.colwise()-pos_sph_agg_k_out.col(i)).colwise().norm().array();
          non_overlapped= {(distance_from_this_sph-2*a >= 0)(i) == false};
          for(int j= 0; j< num_sph; ++j){
              if(j != i) {
                non_overlapped= non_overlapped && ((distance_from_this_sph-2*a > 0)(j) == true);
              }
          }
         //cout << i << ", " << (distance_from_this_sph-(2*a-buf_factor*tol) >= 0).transpose() << endl ;
          min_diff_from2a(i)=((pos_sph_agg_k_out.colwise()-pos_sph_agg_k_out.col(i)).colwise().norm().array()-2*a).array().abs().minCoeff();
      }
      bool attached_at_least_1pair {min_diff_from2a.maxCoeff() < tol};
      string non_overlapped_test= {non_overlapped ? "Success" : "Failed"};
      string attached_at_least_1pair_test= {attached_at_least_1pair ? "Success" : "Failed"};
      cout << k << "th level, non_overlapped_check         : " << non_overlapped_test << endl;
      cout << k << "th level, attached_at_least_1pair_check: " << attached_at_least_1pair_test << endl;

      non_overlapped_all = non_overlapped_all && non_overlapped;
      attached_at_least_1pair_all = attached_at_least_1pair_all && attached_at_least_1pair;
  }

  if(non_overlapped_all && attached_at_least_1pair_all){
    return pos_sph_agg_output;
  }else{
    cout << "Geometry check failed! Please change tol." << endl;
    exit(1);
  }

}


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

// Copyright (c) 2016-2017 Oracle and/or its affiliates.
// Contributed and/or modified by Vissarion Fisikopoulos, on behalf of Oracle
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_AREA_HPP
#define BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_AREA_HPP

#include <boost/geometry/core/srs.hpp>

#include <boost/geometry/formulas/area_formulas.hpp>
#include <boost/geometry/formulas/flattening.hpp>

#include <boost/geometry/strategies/geographic/parameters.hpp>

#include <boost/math/special_functions/atanh.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace area
{

/*!
\brief Geographic area calculation
\ingroup strategies
\details Geographic area calculation by trapezoidal rule plus integral
         approximation that gives the ellipsoidal correction
\tparam PointOfSegment \tparam_segment_point
\tparam FormulaPolicy Formula used to calculate azimuths
\tparam SeriesOrder The order of approximation of the geodesic integral
\tparam Spheroid The spheroid model
\tparam CalculationType \tparam_calculation
\author See
- Danielsen JS, The area under the geodesic. Surv Rev 30(232): 61–66, 1989
- Charles F.F Karney, Algorithms for geodesics, 2011 https://arxiv.org/pdf/1109.4448.pdf

\qbk{
[heading See also]
[link geometry.reference.algorithms.area.area_2_with_strategy area (with strategy)]
}
*/
template
<
    typename PointOfSegment,
    typename FormulaPolicy = strategy::andoyer,
    std::size_t SeriesOrder = strategy::default_order<FormulaPolicy>::value,
    typename Spheroid = srs::spheroid<double>,
    typename CalculationType = void
>
class geographic
{
    // Switch between two kinds of approximation(series in eps and n v.s.series in k ^ 2 and e'^2)
    static const bool ExpandEpsN = true;
    // LongSegment Enables special handling of long segments
    static const bool LongSegment = false;

    //Select default types in case they are not set

    typedef typename boost::mpl::if_c
    <
        boost::is_void<CalculationType>::type::value,
        typename select_most_precise
            <
                typename coordinate_type<PointOfSegment>::type,
                double
            >::type,
        CalculationType
    >::type CT;

protected :
    struct spheroid_constants
    {
        Spheroid m_spheroid;
        CT const m_a2;  // squared equatorial radius
        CT const m_e2;  // squared eccentricity
        CT const m_ep2; // squared second eccentricity
        CT const m_ep;  // second eccentricity
        CT const m_c2;  // squared authalic radius

        inline spheroid_constants(Spheroid const& spheroid)
            : m_spheroid(spheroid)
            , m_a2(math::sqr(get_radius<0>(spheroid)))
            , m_e2(formula::flattening<CT>(spheroid)
                 * (CT(2.0) - CT(formula::flattening<CT>(spheroid))))
            , m_ep2(m_e2 / (CT(1.0) - m_e2))
            , m_ep(math::sqrt(m_ep2))
            , m_c2(authalic_radius(spheroid, m_a2, m_e2))
        {}
    };

    static inline CT authalic_radius(Spheroid const& sph, CT const& a2, CT const& e2)
    {
        CT const c0 = 0;

        if (math::equals(e2, c0))
        {
            return a2;
        }

        CT const sqrt_e2 = math::sqrt(e2);
        CT const c2 = 2;

        return (a2 / c2) +
                  ((math::sqr(get_radius<2>(sph)) * boost::math::atanh(sqrt_e2))
                   / (c2 * sqrt_e2));
    }

    struct area_sums
    {
        CT m_excess_sum;
        CT m_correction_sum;

        // Keep track if encircles some pole
        std::size_t m_crosses_prime_meridian;

        inline area_sums()
            : m_excess_sum(0)
            , m_correction_sum(0)
            , m_crosses_prime_meridian(0)
        {}
        inline CT area(spheroid_constants spheroid_const) const
        {
            CT result;

            CT sum = spheroid_const.m_c2 * m_excess_sum
                   + spheroid_const.m_e2 * spheroid_const.m_a2 * m_correction_sum;

            // If encircles some pole
            if (m_crosses_prime_meridian % 2 == 1)
            {
                std::size_t times_crosses_prime_meridian
                        = 1 + (m_crosses_prime_meridian / 2);

                result = CT(2.0)
                         * geometry::math::pi<CT>()
                         * spheroid_const.m_c2
                         * CT(times_crosses_prime_meridian)
                         - geometry::math::abs(sum);

                if (geometry::math::sign<CT>(sum) == 1)
                {
                    result = - result;
                }

            }
            else
            {
                result = sum;
            }

            return result;
        }
    };

public :
    typedef CT return_type;
    typedef PointOfSegment segment_point_type;
    typedef area_sums state_type;

    explicit inline geographic(Spheroid const& spheroid = Spheroid())
        : m_spheroid_constants(spheroid)
    {}

    inline void apply(PointOfSegment const& p1,
                      PointOfSegment const& p2,
                      area_sums& state) const
    {

        if (! geometry::math::equals(get<0>(p1), get<0>(p2)))
        {

            typedef geometry::formula::area_formulas
                <
                    CT, SeriesOrder, ExpandEpsN
                > area_formulas;

            typename area_formulas::return_type_ellipsoidal result =
                     area_formulas::template ellipsoidal<FormulaPolicy::template inverse>
                                             (p1, p2, m_spheroid_constants);

            state.m_excess_sum += result.spherical_term;
            state.m_correction_sum += result.ellipsoidal_term;

            // Keep track whenever a segment crosses the prime meridian
            geometry::formula::area_formulas<CT>
                    ::crosses_prime_meridian(p1, p2, state);
        }
    }

    inline return_type result(area_sums const& state) const
    {
        return state.area(m_spheroid_constants);
    }

private:
    spheroid_constants m_spheroid_constants;

};

#ifndef DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

namespace services
{

template <typename Point>
struct default_strategy<geographic_tag, Point>
{
    typedef strategy::area::geographic<Point> type;
};

#endif // DOXYGEN_NO_STRATEGY_SPECIALIZATIONS

}

}} // namespace strategy::area

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_GEOGRAPHIC_AREA_HPP


#include <iostream>

#include "sandbox/external/NanoGUI.h"
#include <nanogui/opengl.h>
#include <nanogui/window.h>
#include <nanogui/layout.h>
#include <nanogui/slider.h>
#include <nanogui/combobox.h>
#include <nanogui/textbox.h>
#include <nanogui/glcanvas.h>
#include <nanogui/opengl.h>
#include <nanogui/glutil.h>
#include "sandbox/SceneNode.h"
#include "sandbox/base/RenderCallback.h"
#include "sandbox/base/Transform.h"
#include "sandbox/base/Camera.h"
#include "sandbox/base/NodeRenderer.h"
#include "sandbox/geometry/algorithms/SmoothNormals.h"
#include "sandbox/geometry/shapes/Cylinder.h"
#include "sandbox/geometry/shapes/Grid.h"
#include "sandbox/geometry/Material.h"
#include "sandbox/graphics/MeshRenderer.h"
#include "sandbox/graphics/shaders/MaterialShader.h"
#include "sandbox/graphics/Viewport.h"
#include "sandbox/graphics/Window.h"
#include "sandbox/graphics/RenderState.h"
#include "sandbox/data/KdTree.h"
#include "glm/glm.hpp"
#include <glm/gtc/matrix_access.hpp>
#include <glm/gtc/matrix_transform.hpp>
#include <glm/gtc/type_ptr.hpp>
#include <glm/gtc/quaternion.hpp>
#include <cstdlib>

#include <iostream>
#include <Eigen/Dense>
#include "src/LeastSquares.h"
#include <algorithm>

using namespace sandbox;

float xFrequency = 1.0;

float function(float x, float y, float t) {
	return 0.5*std::cos(2.0f*3.1415*y)*std::cos(xFrequency*2.0f*3.1415*(x+t));
}

float fx(float x, float y) {
	return 0.5*std::cos(2.0f*3.1415*y)*std::sin(xFrequency*2.0f*3.1415*x)*-xFrequency*2.0f*3.1415;
}

float fy(float x, float y) {
	return 0.5*std::sin(2.0f*3.1415*y)*std::cos(xFrequency*2.0f*3.1415*x)*-xFrequency*3.1415;
}

class TestApp : public NanoguiScreen {
public:

	//TestApp() : NanoguiScreen(Eigen::Vector2i(1180, 980), "Test App") {
	TestApp() : NanoguiScreen(Eigen::Vector2i(1150, 600), "Test App") {
		using namespace nanogui;

		Grid* grid = new Grid(30, 30);
		Grid* estGrid = new Grid(30, 30);

		SceneNode* eventNode = new SceneNode(&scene);
			NanoguiResizeEvent* resize = new NanoguiResizeEvent(this);
			eventNode->addComponent(resize);
			ResizeEventHandler* resizeCallback = (new ResizeCallback())->init(this);
			resize->subscribe(resizeCallback);
			eventNode->addComponent(resizeCallback);
		SceneNode* geometryNode = new SceneNode(&scene);
			SceneNode* arrowNode = new SceneNode(geometryNode);
				arrowNode->addComponent(new Transform(glm::scale(glm::mat4(1.0f),glm::vec3(1.0f/1.25))*glm::translate(glm::mat4(1.0f),glm::vec3(0,2.0,0))));
				SceneNode* arrowTipNode = new SceneNode(arrowNode);
					glm::mat4 tipTrans = glm::translate(glm::mat4(1.0f),glm::vec3(0,0.25,0));
					tipTrans = glm::scale(tipTrans,glm::vec3(0.25f));
					arrowTipNode->addComponent(new Transform(tipTrans));
					arrowTipNode->addComponent(new Cylinder(20, 1.0f, 0.0f));
					arrowTipNode->addComponent(new MeshRenderer());
					Material* material = new Material();
					material->setColor(glm::vec4(1.0f, 0, 0, 1));
					arrowTipNode->addComponent(material);
				SceneNode* cylNode = new SceneNode(arrowNode);
					glm::mat4 cylTrans = glm::translate(glm::mat4(1.0f),glm::vec3(0,-1.0,0));
					cylTrans = glm::scale(cylTrans,glm::vec3(0.1f, 1.0f, 0.1f));
					cylNode->addComponent(new Transform(cylTrans));
					cylNode->addComponent(new Cylinder(20));
					cylNode->addComponent(new MeshRenderer());
					material = new Material();
					material->setColor(glm::vec4(1.0f, 0, 0, 1));
					cylNode->addComponent(material);
			SceneNode* gridNode = new SceneNode(geometryNode);
				gridNode->addComponent(grid);
				gridNode->addComponent(new SmoothNormals());
				gridNode->addComponent(new MeshRenderer());
				gridNode->addComponent(new Material());
			SceneNode* estGridNode = new SceneNode(geometryNode);
				estGridNode->addComponent(estGrid);
				estGridNode->addComponent(new SmoothNormals());
				estGridNode->addComponent(new MeshRenderer());
				estGridNode->addComponent(new Material());
			SceneNode* sampleNode = new SceneNode(geometryNode);
				Mesh* sampleMesh = new Mesh();
				sampleNode->addComponent(sampleMesh);
				sampleNode->addComponent(new MeshRenderer(GL_POINTS));
				Material* sampleMaterial = new Material();
				sampleMaterial->setColor(glm::vec4(1.0f,0,0,1));
				sampleNode->addComponent(sampleMaterial);

		graphicsNode = new SceneNode(&scene);
			graphicsNode->addComponent(new sandbox::Window(eventNode));
			graphicsNode->addComponent((new OpenGLCallback())->init(this));
			SceneNode* functionViewNode = new SceneNode(graphicsNode);
				functionViewNode->addComponent(new PercentViewport(glm::vec4(0.0, 0.0, 0.5, 1.0)));
				functionViewNode->addComponent(new Transform(glm::translate(glm::mat4(1.0f),glm::vec3(0,2.5,5.0))));
				functionViewNode->addComponent(new Camera());
				functionViewNode->addComponent(new MaterialShader());
				SceneNode* functionNode = new SceneNode(functionViewNode);
					functionNode->addComponent(new Transform(glm::rotate(glm::mat4(1.0f), -3.1415f*1.0f/2.0f, glm::vec3(1.0f,0.0,0))*glm::scale(glm::mat4(1.0f), glm::vec3(3.0f,3.0f,1.0f*3.0f))));
					functionNode->addComponent(new NodeRenderer(gridNode));
				SceneNode* pointsNode = new SceneNode(functionViewNode);
					pointsNode->addComponent(new Transform(glm::rotate(glm::mat4(1.0f), -3.1415f*1.0f/2.0f, glm::vec3(1.0f,0.0,0))*glm::scale(glm::mat4(1.0f), glm::vec3(3.0f,3.0f,1.0f*3.0f))));
					//pointsNode->addComponent(new NodeRenderer(sampleNode));
			SceneNode* flatGraph = new SceneNode(graphicsNode);
				flatGraph->addComponent(new PercentViewport(glm::vec4(0.0, 0.5, 0.5, 0.5)));
				flatGraph->addComponent(new Transform(glm::translate(glm::mat4(1.0f),glm::vec3(0,0,3))));
				flatGraph->addComponent(new Camera());
				flatGraph->addComponent(new MaterialShader());
				SceneNode* flatPointsNode = new SceneNode(flatGraph);
					flatPointsNode->addComponent(new Transform(glm::rotate(glm::mat4(1.0f), 3.1415f*4.0f/2.0f, glm::vec3(1.0f,0.0,0))*glm::scale(glm::mat4(1.0f), glm::vec3(2.0f,2.0f,0.0f*2.0f))));
			functionViewNode = new SceneNode(graphicsNode);
				//functionViewNode->addComponent(new PercentViewport(glm::vec4(0.5, 0.0, 0.5, 0.5)));
				functionViewNode->addComponent(new PercentViewport(glm::vec4(0.5, 0.0, 0.5, 1.0)));
				functionViewNode->addComponent(new Transform(glm::translate(glm::mat4(1.0f),glm::vec3(0,2.5,5.0))));
				functionViewNode->addComponent(new Camera());
				functionViewNode->addComponent(new MaterialShader());
				functionNode = new SceneNode(functionViewNode);
					functionNode->addComponent(new Transform(glm::rotate(glm::mat4(1.0f), -3.1415f/2.0f, glm::vec3(1.0f,0.0,0))*glm::scale(glm::mat4(1.0f), glm::vec3(3.0f))));
					functionNode->addComponent(new NodeRenderer(estGridNode));
				SceneNode* estPointsNode = new SceneNode(functionViewNode);
					estPointsNode->addComponent(new Transform(glm::rotate(glm::mat4(1.0f), -3.1415f*1.0f/2.0f, glm::vec3(1.0f,0.0,0))*glm::scale(glm::mat4(1.0f), glm::vec3(3.0f,3.0f,1.0f*3.0f))));
			SceneNode* estFlatGraph = new SceneNode(graphicsNode);
				estFlatGraph->addComponent(new PercentViewport(glm::vec4(0.5, 0.5, 0.5, 0.5)));
				estFlatGraph->addComponent(new Transform(glm::translate(glm::mat4(1.0f),glm::vec3(0,0,3))));
				estFlatGraph->addComponent(new Camera());
				estFlatGraph->addComponent(new MaterialShader());
				SceneNode* estFlatPointsNode = new SceneNode(estFlatGraph);
					estFlatPointsNode->addComponent(new Transform(glm::rotate(glm::mat4(1.0f), 3.1415f*4.0f/2.0f, glm::vec3(1.0f,0.0,0))*glm::scale(glm::mat4(1.0f), glm::vec3(2.0f,2.0f,0.0f*2.0f))*glm::rotate(glm::mat4(1.0f), 3.1415f*0.0f/2.0f, glm::vec3(0.0f,0.0f,1.0f))));

		float time = 0.25;

		for (int x = 0; x < grid->getWidth(); x++) {
			for (int y = 0; y < grid->getHeight(); y++) {
				grid->getNode(x,y).z = function(1.0f*x/(grid->getWidth()-1), 1.0f*y/(grid->getHeight()-1), time);
			}
		}

		int numSamples = 4000;
		std::vector<glm::vec4> samplePoints;

		/*std::vector<int> xDim;
		for (int f = 0; f < 10; f++) {
			cards.push_back(f);
		}
		std::random_shuffle(cards.begin(), cards.end());
		for (int f = 0; f < 10; f++) {
			std::cout << cards[f] << std::endl;
		}*/

		std::vector<float> xDim;
		std::vector<float> yDim;
		std::vector<float> tDim;

		int numSplits = 10;//grid->getWidth();
		float step = 1.0f/numSplits;

		for (int f = 0; f < numSamples/numSplits; f++) {
			for (int i = 0; i < numSplits; i++) {
				xDim.push_back(step*i + step*float(std::rand())/RAND_MAX);
				yDim.push_back(step*i + step*float(std::rand())/RAND_MAX);
				tDim.push_back(step*i + step*float(std::rand())/RAND_MAX);
			}
			std::random_shuffle(xDim.begin(), xDim.end());
			std::random_shuffle(yDim.begin(), yDim.end());
			std::random_shuffle(tDim.begin(), tDim.end());

			for (int i = 0; i < numSplits; i++) {
				float x = xDim[i];
				float y = yDim[i];
				float t = tDim[i];
				float z = function(x,y,t);
				samplePoints.push_back(glm::vec4(x,y,t,z));
			}
			/* random sampling
			float x = float(std::rand())/RAND_MAX;
			float y = float(std::rand())/RAND_MAX;
			float t = float(std::rand())/RAND_MAX;
			float z = function(x,y,t);
			samplePoints.push_back(glm::vec4(x,y,t,z));*/
		}

		for (int iteration = 0; iteration < 1; iteration++) {

			PointCollection pc(samplePoints);

			for (int x = 0; x < grid->getWidth(); x++) {
				for (int y = 0; y < grid->getHeight(); y++) {
					//estGrid->getNode(x,y).z = function(1.0f*x/(grid->getWidth()-1), 1.0f*y/(grid->getHeight()-1));
					float residual;
					estGrid->getNode(x,y).z = calculateFromSamples(pc, glm::vec3(1.0f*x/(grid->getWidth()-1), 1.0f*y/(grid->getHeight()-1), time), &residual);
					estGrid->getCoord(x,y).x = residual;
				}
			}

			glm::vec2 minMax;
			for (int f = 0; f < pc.getPoints().size(); f++) {
				if (f == 0) {
					minMax = glm::vec2(pc.getEstResiduals()[f]);
				}
				else {
					minMax.x = minMax.x < pc.getEstResiduals()[f] ? minMax.x : pc.getEstResiduals()[f];
					minMax.y = minMax.y > pc.getEstResiduals()[f] ? minMax.y : pc.getEstResiduals()[f];
				}

			}
			std::cout << minMax.x << ", "<< minMax.y << std::endl;

			for (int x = 0; x < grid->getWidth(); x++) {
				for (int y = 0; y < grid->getHeight(); y++) {
					estGrid->getCoord(x,y).x = (estGrid->getCoord(x,y).x - minMax.x)/(minMax.y-minMax.x);
				}
			}

			std::vector<float> splits;
			std::vector<int> splitSizes;
			for (int f = 0; f < numSplits*numSplits*numSplits; f++) {
				splits.push_back(0.0f);
				splitSizes.push_back(0);
			}

			for (int f = 0; f < pc.getPoints().size(); f++) {
				glm::vec4 splitIndex = pc.getActualPoints()[f];
				splitIndex = splitIndex / step;
				int index = (int(splitIndex.x)*numSplits+int(splitIndex.y))*numSplits+int(splitIndex.z);
				splits[index] += pc.getEstResiduals()[f];
				splitSizes[index]++;
			}

			float totalResidual = 0.0f;
			for (int f = 0; f < splits.size(); f++) {
				if (splitSizes[f] > 0) {
					splits[f] = splits[f]/splitSizes[f];
				}
				totalResidual += splits[f];
			}

			int numS = 0;
			for (int f = 0; f < splits.size(); f++) {
				splits[f] /= totalResidual;
				//std::cout << splits[f]*numSamples << std::endl;
				numS += std::round(splits[f]*numSamples);
			}

			//std::cout << numS << std::endl;

			/*for (int x = 0; x < numSplits; x++) {
				for (int y = 0; y < numSplits; y++) {
					for (int t = 0; t < numSplits; t++) {

					}
				}
			}*/

			for (int x = 0; x < numSplits; x++) {
				for (int y = 0; y < numSplits; y++) {
					for (int t = 0; t < numSplits; t++) {
						int index = (x*numSplits+y)*numSplits+t;
						for (int f = 0; f < std::round(1.0f*splits[index]*numSamples); f++) {
							float x2 = 1.0f*step*x + step*float(std::rand())/RAND_MAX;
							float y2 = 1.0f*step*y + step*float(std::rand())/RAND_MAX;
							float t2 = 1.0f*step*t + step*float(std::rand())/RAND_MAX;
							float z2 = function(x2,y2,t2);
							//std::cout << samplePoints.size() << " " << x << " " << x2 << " " << y2 << " " << t2 << " " << z2 << " " << splits[index] << " " << step << std::endl;
							samplePoints.push_back(glm::vec4(x2,y2,t2,z2));
						}
					}
				}
			}

			// vector estimate
			/*std::vector<glm::vec4> newPoints;
			for (int f = 0; f < pc.getPoints().size(); f++) {
				float randPercent = 2.0f*(float(std::rand())/RAND_MAX - 0.5);
				glm::vec4 newPoint = pc.getActualPoints()[f] + glm::vec4(pc.getEstGradients()[f]*randPercent,0.0);
				newPoint.w = function(newPoint.x, newPoint.y, newPoint.z);
				newPoints.push_back(newPoint);
			}

			for (int f = 0; f < newPoints.size(); f++) {
				samplePoints.push_back(newPoints[f]);
			}*/

			/*for (int f = 0; f < numSamples; f++) {
				float x = float(std::rand())/RAND_MAX;
				float y = float(std::rand())/RAND_MAX;
				float t = float(std::rand())/RAND_MAX;
				float z = function(x,y,t);
				samplePoints.push_back(glm::vec4(x,y,t,z));
			}*/

			/*float totalResidual = 0.0f;
			float dr = 1.0f/pc.getPoints().size();
			for (int f = 0; f < pc.getPoints().size(); f++) {
				totalResidual += (pc.getEstResiduals()[f]-minMax.x)/(pc.getEstResiduals()[f]-minMax.y - pc.getEstResiduals()[f]-minMax.x);
			}*/

			/*splits.clear();
			totalResidual = 0.0f;
			for (int x = 0; x < numSplits; x++) {
				for (int y = 0; y < numSplits; y++) {
					for (int t = 0; t < numSplits; t++) {
						float newX = 1.0f*step*x + 0.5*step;
						float newY = 1.0f*step*y + 0.5*step;
						float newT = 1.0f*step*t + 0.5*step;
						//estGrid->getNode(x,y).z = function(1.0f*x/(grid->getWidth()-1), 1.0f*y/(grid->getHeight()-1));

						int numNearest = 1;
						std::vector<float> point;
						point.push_back(newX);
						point.push_back(newY);
						point.push_back(newT);
						std::vector<KdTree<float>::KdValue> nearest = pc.getKdTree().getNearestSorted(point, numNearest);

						float residual = pc.getEstResiduals()[nearest[0].index];
						//std::cout << newX << " " << newY << " " << newT << " " << pc.getEstResiduals()[nearest[0].index] << std::endl;
						splits.push_back(residual);
						totalResidual += residual;
					}
				}
			}

			for (int f = 0; f < splits.size(); f++) {
				splits[f] /= totalResidual;
			}

			int splitIndex = 0;
			for (int x = 0; x < numSplits; x++) {
				for (int y = 0; y < numSplits; y++) {
					for (int t = 0; t < numSplits; t++) {
						for (int f = 0; f < int(splits[splitIndex]*numSamples); f++) {
							float newX = 1.0f*step*x + step*float(std::rand())/RAND_MAX;
							float newY = 1.0f*step*y + step*float(std::rand())/RAND_MAX;
							float newT = 1.0f*step*t + step*float(std::rand())/RAND_MAX;
							float newZ = function(newX, newY, newT);
							samplePoints.push_back(glm::vec4(newX, newY, newT, newZ));
						}
						splitIndex++;
					}
				}
			}

			/*for (int f = 0; f < numSamples; f++) {
				float x = float(std::rand())/RAND_MAX;
				float y = float(std::rand())/RAND_MAX;
				float t = float(std::rand())/RAND_MAX;
				float z = function(x,y,t);
				samplePoints.push_back(glm::vec4(x,y,t,z));
			}*/
			std::cout << samplePoints.size() << std::endl;
		}

    	resizeEvent(Eigen::Vector2i(width(), height()));
	}

	class PointCollection : public KdSearchable<float> {
	public:

		class ObjectiveFunction {
		public:
			virtual ~ObjectiveFunction() {}
			virtual float getValue(int sampleNum, const glm::vec4& point) const {
				return point.w;
			}
		};

		class GradObjectiveFunction : public ObjectiveFunction {
		public:
			GradObjectiveFunction(int dim, PointCollection* pc) : dim(dim), pc(pc) {}
			virtual ~GradObjectiveFunction() {}
			virtual float getValue(int sampleNum, const glm::vec4& point) const {
				return pc->estGradients[sampleNum][dim];
			}
		private:
			int dim;
			PointCollection* pc;
		};

		const glm::vec3 calculateGradient(int numNearest, glm::vec4 estPoint, float value, bool pointIsSample, const ObjectiveFunction* objFunction, float* residual = NULL) const {
			int startNearest = pointIsSample ? 1 : 0;

			Eigen::MatrixXf A = Eigen::MatrixXf(numNearest-startNearest, 3);
			Eigen::VectorXf b = Eigen::VectorXf(numNearest-startNearest);

			std::vector<float> point;
			point.push_back(estPoint[0]);
			point.push_back(estPoint[1]);
			point.push_back(estPoint[2]);
			//point.push_back(estPoint[3]);

			std::vector<KdTree<float>::KdValue> nearest = kdTree->getNearestSorted(point, numNearest);

			for (int f = startNearest; f < nearest.size(); f++) {
				glm::vec3 diff = glm::vec3(points[nearest[f].index]-estPoint);
				glm::vec3 dir = normalize(diff);
				float dirDeriv = (objFunction->getValue(nearest[f].index, points[nearest[f].index])-value)/(glm::length(diff));

				Eigen::VectorXf d(3);
				d[0] = dir[0];
				d[1] = dir[1];
				d[2] = dir[2];

				b[f-startNearest] = dirDeriv;
				A.block(f-startNearest, 0, 1, 3) = d.transpose();
			}

			Eigen::VectorXf sol = calculateLeastSquares(A,b);
			if (residual) {
				*residual = (A * sol - b).norm();
			}

			return glm::vec3(sol[0], sol[1], sol[2]);
		}

		PointCollection(const std::vector<glm::vec4>& points) : points(points) {
			for (int f = 0; f < points.size(); f++) {
				indices.push_back(f);
			}

			std::vector<unsigned int> dimensions;
			dimensions.push_back(0);
			dimensions.push_back(1);
			dimensions.push_back(2);
			//dimensions.push_back(3);
			kdTree = new KdTree<float>(dimensions, *this, new EuclideanDistance<float>());

			int numNearest = 11;
//			Eigen::MatrixXf A = Eigen::MatrixXf(numNearest-1, 3);
//			Eigen::VectorXf b = Eigen::VectorXf(numNearest-1);

			ObjectiveFunction fun;

			for (int sampleNum = 0; sampleNum < points.size(); sampleNum++) {
				float residual;
				glm::vec3 gradient = calculateGradient(numNearest, points[sampleNum], points[sampleNum].w, true, &fun, &residual);
				estGradients.push_back(gradient);
				estResiduals.push_back(residual);

				/*std::vector<float> point;
				point.push_back(points[sampleNum][0]);
				point.push_back(points[sampleNum][1]);
				point.push_back(points[sampleNum][2]);
				point.push_back(points[sampleNum][3]);
				std::vector<KdTree<float>::KdValue> nearest = kdTree->getNearestSorted(point, numNearest);

				for (int f = 1; f < nearest.size(); f++) {
					glm::vec4 diff = points[nearest[f].index]-points[sampleNum];
					glm::vec3 dir = normalize(glm::vec3(diff));
					float dirDeriv = diff.w/(glm::length(glm::vec3(diff)));

					Eigen::VectorXf d(3);
					d[0] = dir[0];
					d[1] = dir[1];
					d[2] = dir[2];

					b[f-1] = dirDeriv;
					A.block(f-1, 0, 1, 3) = d.transpose();
				}

				Eigen::VectorXf sol = calculateLeastSquares(A,b);
				float residual = (A * sol - b).norm();
				estGradients.push_back(glm::vec3(sol[0], sol[1], sol[2]));
				estResiduals.push_back(residual);*/
			}

			// calculate Hessians
			std::vector<GradObjectiveFunction> dimFunctions;
			dimFunctions.push_back(GradObjectiveFunction(0, this));
			dimFunctions.push_back(GradObjectiveFunction(1, this));
			dimFunctions.push_back(GradObjectiveFunction(2, this));
			for (int sampleNum = 0; sampleNum < points.size(); sampleNum++) {
				glm::mat3 hessian(1.0f);
				for (int f = 0; f < 3; f++) {
					glm::vec3 gradient = calculateGradient(numNearest, points[sampleNum], estGradients[sampleNum][f], true, &(dimFunctions[f]));
					hessian = glm::row(hessian, f, gradient);
					//std::cout << "Grad: " << f << " "  << gradient[0] << " " << gradient[1] << " " << gradient[2] << std::endl;
					//std::cout << "Hessian: " << f << " "  << glm::row(hessian, f)[0] << " " << glm::row(hessian, f)[1] << " " << glm::row(hessian, f)[2] << std::endl;
				}

				estHessians.push_back(hessian);
			}

			dimensions.clear();
			dimensions.push_back(0);
			dimensions.push_back(1);
			dimensions.push_back(2);
			queryKdTree = new KdTree<float>(dimensions, *this, new EuclideanDistance<float>());

		}
		~PointCollection() {
			delete kdTree;
			delete queryKdTree;
		}

		const std::vector<unsigned int>& getPoints() const { return indices; }
		float getDimension(unsigned int index, unsigned int dimension) const { return points[index][dimension]; }
		const std::vector<glm::vec4>& getActualPoints() const { return points; }
		const std::vector<glm::vec3>& getEstGradients() const { return estGradients; }
		const std::vector<float>& getEstResiduals() const { return estResiduals; }
		const std::vector<glm::mat3>& getEstHessians() const { return estHessians; }
		const KdTree<float>& getKdTree() const { return *queryKdTree; }
	private:
		std::vector<glm::vec4> points;
		std::vector<unsigned int> indices;
		std::vector<glm::vec3> estGradients;
		std::vector<float> estResiduals;
		std::vector<glm::mat3> estHessians;
		KdTree<float>* kdTree;
		KdTree<float>* queryKdTree;
	};

	float calculateFromSamples(const PointCollection& pc, const glm::vec3& pos, float* residual) {
		//return 0.0f;
				/*if (true) {//recalculateZ_FirstHalf) { // nearest directional deriv
					numAlgorithms++;
					glm::vec3 diff = samplePoints[sampleNum]-samplePoints[nearest[0].index];
					glm::vec2 dir = normalize(glm::vec2(diff));
					float dirDeriv = glm::dot(estGradients[nearest[0].index],dir);
					zEstimate = samplePoints[nearest[0].index].z + dirDeriv*glm::length(glm::vec2(diff));

					finalEstimate += zEstimate;
				}*/
		int numNearest = 10;

		std::vector<float> point;
		point.push_back(pos[0]);
		point.push_back(pos[1]);
		point.push_back(pos[2]);

		std::vector<KdTree<float>::KdValue> nearest = pc.getKdTree().getNearestSorted(point, numNearest);

		glm::vec3 diff = pos-glm::vec3(pc.getActualPoints()[nearest[0].index]);
		glm::vec3 dir = normalize(diff);
		float dirDeriv = glm::dot(pc.getEstGradients()[nearest[0].index],dir);

		*residual = pc.getEstResiduals()[nearest[0].index];

		//return pc.getActualPoints()[nearest[0].index].w + dirDeriv*glm::length(diff);

		glm::vec3 gradient = pc.getEstGradients()[nearest[0].index];
		glm::vec4 newPoint = pc.getActualPoints()[nearest[0].index];

		float integrationSteps = 1.0f;
		glm::vec3 intDiff = diff/integrationSteps;
		PointCollection::ObjectiveFunction fun;

		for (int f = 0; f < integrationSteps; f++) {
			//float hessCalc =  0.5*glm::dot(diff, pc.getEstHessians()[nearest[0].index]*diff)
			newPoint += glm::vec4(intDiff, glm::dot(gradient, intDiff));

			gradient = pc.calculateGradient(5, newPoint, newPoint.w, false, &fun);
		}

		//return newPoint.w;

		//return pc.getActualPoints()[nearest[0].index].w;

		return pc.getActualPoints()[nearest[0].index].w + glm::dot(pc.getEstGradients()[nearest[0].index],diff);

		//return f(x+dx) ~= f(x) + df(x)*dx + (1/2)*dx*H(x)*dx
		return pc.getActualPoints()[nearest[0].index].w + glm::dot(pc.getEstGradients()[nearest[0].index],diff)
		 + 0.5*glm::dot(diff, pc.getEstHessians()[nearest[0].index]*diff);

		// mean gradient
		float value = 0.0f;
		for (int f = 0; f < nearest.size(); f++) {
			diff = pos-glm::vec3(pc.getActualPoints()[nearest[f].index]);
			value += (pc.getActualPoints()[nearest[f].index].w + glm::dot(pc.getEstGradients()[nearest[f].index],diff));
		}
		return value/nearest.size();

		// inverse weighted
		float totalWeight = 0.0f;
		for (int f = 0; f < nearest.size(); f++) {
			totalWeight += 1.0f/nearest[f].distance;
		}

		// inverse weighted gradient
		value = 0.0f;
		for (int f = 0; f < nearest.size(); f++) {
			diff = pos-glm::vec3(pc.getActualPoints()[nearest[f].index]);
			value += (pc.getActualPoints()[nearest[f].index].w + glm::dot(pc.getEstGradients()[nearest[f].index],diff)) * ((1.0f/nearest[f].distance)/totalWeight);
		}
		return value;

		// inverse weighted distance
		value = 0.0f;
		for (int f = 0; f < nearest.size(); f++) {
			value += pc.getActualPoints()[nearest[f].index].w * ((1.0f/nearest[f].distance)/totalWeight);
		}
		return value;
	}

	glm::quat rotationBetweenVectors(glm::vec3 start, glm::vec3 dest){
		start = normalize(start);
		dest = normalize(dest);

		float cosTheta = dot(start, dest);
		glm::vec3 rotationAxis;

		if (cosTheta < -1 + 0.001f){
			// special case when vectors in opposite directions:
			// there is no "ideal" rotation axis
			// So guess one; any will do as long as it's perpendicular to start
			rotationAxis = glm::cross(glm::vec3(0.0f, 0.0f, 1.0f), start);
			if (glm::length(rotationAxis) < 0.01 ) // bad luck, they were parallel, try again!
				rotationAxis = glm::cross(glm::vec3(1.0f, 0.0f, 0.0f), start);

			rotationAxis = glm::normalize(rotationAxis);
			return glm::angleAxis(glm::radians(180.0f), rotationAxis);
		}

		rotationAxis = glm::cross(start, dest);

		float s = sqrt( (1+cosTheta)*2 );
		float invs = 1 / s;

		return glm::quat(
			s * 0.5f,
			rotationAxis.x * invs,
			rotationAxis.y * invs,
			rotationAxis.z * invs
		);

	}

	~TestApp() {
	}

	void drawContents() {
		scene.updateModel();
		scene.updateSharedContext(context);
		scene.updateContext(context);
		graphicsNode->render(context);
	}

private:

	class ResizeCallback : public StateEventHandlerCallback<ResizeState, TestApp> {
		void onEvent(const ResizeState& state, TestApp* app) {
			std::cout << state.width << " " << state.height << " " << app << std::endl;
		}
	};

	class OpenGLCallback : public RenderCallback<TestApp> {
		void renderCallback(const SceneContext& sceneContext, TestApp* app) {
			//std::cout << "Clear screen" << std::endl;
			glClearColor(0.75,0.75,0.75,1);
            //glClearDepth(1.0f);
			glPointSize(10);
			glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_STENCIL_BUFFER_BIT);
			glEnable(GL_DEPTH_TEST);
            glDepthFunc(GL_LESS);
            //glDepthFunc(GL_LEQUAL);
            glPatchParameteri(GL_PATCH_VERTICES, 3);
            glEnable(GL_CULL_FACE);
            glDisable(GL_BLEND);
            glCullFace(GL_BACK);
            glDisable(GL_CULL_FACE);
		}
	};

	class TestCallback : public RenderCallback<TestApp> {
	public:
		TestCallback(std::string name) : name(name) {}
		void renderCallback(const SceneContext& sceneContext, TestApp* app) {
			RenderState& renderState = RenderState::get(sceneContext);
			glm::mat4 model = renderState.getModelMatrix().get();
			glm::vec3 v(1.0);
			//glm::vec3 v(0.0f);
			v = model*glm::vec4(v, 1.0);
			std::cout << name << ": " << v.x << " " << v.y << " " << v.z << std::endl;
		}
		std::string name;
	};

	SceneContext context;
	SceneNode scene;
	SceneNode* graphicsNode;
};

int main(int argc, char**argv) {
	nanogui::init();

	nanogui::Screen* screen = new TestApp();

	screen->performLayout();
	screen->drawAll();
	screen->setVisible(true);

	nanogui::mainloop();
	nanogui::shutdown();

	return 0;
}


#include <iostream>
#include <cassert>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

const int debug_level = 0;

#define DEBUG(min_level, x)      \
  if (debug_level >= min_level)  \
  {                              \
    std::cerr << x << std::endl; \
  }

typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> GraphTraits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
                              boost::property<boost::edge_capacity_t, long,
                                              boost::property<boost::edge_residual_capacity_t, long,
                                                              boost::property<boost::edge_reverse_t, GraphTraits::edge_descriptor>>>>
    Graph;
typedef Graph::vertex_descriptor Vertex;

template <typename T>
void print_vec(std::vector<T> &vec, std::ostream &stream)
{
  for (int i = 0; i < int(vec.size()); i++)
  {
    if (i > 0)
    {
      stream << " ";
    }
    stream << vec.at(i);
  }
  stream << "\n";
}

void testcase()
{
  int l, p;
  std::cin >> l >> p;
  assert(l >= 1 && l <= 500 && p >= 1 && p <= l * l);

  std::vector<int> start_pops_by_town(l), end_pops_by_town(l);
  int total_start_pops = 0, total_end_pops = 0;
  for (int i = 0; i < l; i++)
  {
    int g, d;
    std::cin >> g >> d;
    assert(g >= 1 && g <= 1e6 && d >= 1 && d <= 1e6);
    start_pops_by_town.at(i) = g;
    total_start_pops += g;
    end_pops_by_town.at(i) = d;
    total_end_pops += d;
  }
  DEBUG(2, "total_start_pops " << total_start_pops << " total_end_pops " << total_end_pops);

  std::vector<int> starts_by_path(p), ends_by_path(p), min_caps_by_path(p), max_caps_by_path(p);
  for (int i = 0; i < p; i++)
  {
    int f, t, c, C;
    std::cin >> f >> t >> c >> C;
    assert(f >= 0 && f < l && t >= 0 && t < l);
    assert(c >= 0 && c <= C && C <= 1e6);
    starts_by_path.at(i) = f;
    ends_by_path.at(i) = t;
    min_caps_by_path.at(i) = c;
    max_caps_by_path.at(i) = C;
  }

  if (total_end_pops > total_start_pops)
  {
    std::cout << "no\n";
    return;
  }

  int num_nodes = l + p + 2, next_free_node = 0;
  Vertex source = next_free_node++;
  Vertex sink = next_free_node++;
  auto town_at = [l, next_free_node](int i) { assert(i >= 0 && i < l);  return next_free_node + i; };
  next_free_node += l;
  auto path_mid_node_at = [p, next_free_node](int i) { assert(i >= 0 && i < p);  return next_free_node + i; };
  next_free_node += p;
  assert(next_free_node == num_nodes);
  Graph G(num_nodes);

  auto add_edge = [&G](int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0;
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  };

  std::vector<int> demands_by_town(l);
  for (int i = 0; i < l; i++)
  {
    demands_by_town.at(i) = end_pops_by_town.at(i) - start_pops_by_town.at(i);
  }
  if (debug_level >= 2)
  {
    std::cerr << "demands_by_town before adjustment ";
    print_vec(demands_by_town, std::cerr);
  }

  for (int i = 0; i < p; i++)
  {
    int s = starts_by_path.at(i), e = ends_by_path.at(i), mid = path_mid_node_at(i), min = min_caps_by_path.at(i), max = max_caps_by_path.at(i);
    demands_by_town.at(s) += min;
    demands_by_town.at(e) -= min;
    add_edge(town_at(s), mid, max - min);
    add_edge(mid, town_at(e), max - min);
  }
  if (debug_level >= 2)
  {
    std::cerr << "demands_by_town after adjustment ";
    print_vec(demands_by_town, std::cerr);
  }

  for (int i = 0; i < l; i++)
  {
    int d = demands_by_town.at(i);
    if (d > 0)
    {
      add_edge(town_at(i), sink, d);
    }
    else if (d < 0)
    {
      add_edge(source, town_at(i), -d);
    }
  }

  int target_flow = 0;
  for (int d : demands_by_town)
  {
    if (d > 0)
    {
      target_flow += d;
    }
  }
  DEBUG(2, "target_flow " << target_flow);

  long flow = boost::push_relabel_max_flow(G, source, sink);
  DEBUG(2, "flow " << flow);
  assert(flow >= 0 && flow <= target_flow);
  std::cout << (flow == target_flow ? "yes\n" : "no\n");
}

int main()
{
  std::ios_base::sync_with_stdio(false);

  int t;
  std::cin >> t;
  for (int i = 0; i < t; i++)
  {
    testcase();
  }

  return 0;
}


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

#include <boost/test/unit_test.hpp>

#include "Werk/Math/SimpleLinearRegression.hpp"

BOOST_AUTO_TEST_SUITE(SimpleLinearRegressionTest)

BOOST_AUTO_TEST_CASE(TestEmpty)
{
	werk::SimpleLinearRegression r;
	BOOST_REQUIRE_EQUAL(r.count(), 0);
	BOOST_REQUIRE(std::isnan(r.beta()));
	BOOST_REQUIRE(std::isnan(r.correlation()));
	BOOST_REQUIRE(std::isnan(r.predict(2.0)));
}

BOOST_AUTO_TEST_CASE(TestLinear)
{
	werk::SimpleLinearRegression r;

	r.sample(1.0, 4.0);
	BOOST_REQUIRE_EQUAL(r.count(), 1);
	BOOST_REQUIRE(std::isnan(r.beta()));
	BOOST_REQUIRE(std::isnan(r.correlation()));
	BOOST_REQUIRE(std::isnan(r.predict(2.0)));

	r.sample(3.0, 8.0);
	BOOST_REQUIRE_EQUAL(r.count(), 2);
	BOOST_REQUIRE_EQUAL(r.beta(), 2.0);
	BOOST_REQUIRE_EQUAL(r.correlation(), 1.0);
	BOOST_REQUIRE_EQUAL(r.predict(2.0), 6.0);

	r.sample(5.0, 12.0);
	BOOST_REQUIRE_EQUAL(r.count(), 3);
	BOOST_REQUIRE_EQUAL(r.correlation(), 1.0);
	BOOST_REQUIRE_CLOSE(r.beta(), 2.0, 0.000000001);
	BOOST_REQUIRE_CLOSE(r.alpha(), 2.0, 0.000000001);
	BOOST_REQUIRE_CLOSE(r.predict(2.0), 6.0, 0.000000001);

	r.sample(6.0, 14.0);
	BOOST_REQUIRE_EQUAL(r.count(), 4);
	BOOST_REQUIRE_EQUAL(r.correlation(), 1.0);
	BOOST_REQUIRE_CLOSE(r.beta(), 2.0, 0.000000001);
	BOOST_REQUIRE_CLOSE(r.alpha(), 2.0, 0.000000001);
	BOOST_REQUIRE_CLOSE(r.predict(2.0), 6.0, 0.000000001);

	r.reset();
	BOOST_REQUIRE_EQUAL(r.count(), 0);
}

BOOST_AUTO_TEST_CASE(TestZeroCorrelation)
{
	werk::SimpleLinearRegression r;

	r.sample(-1.0, 3.0);
	r.sample(0.0, 0.0);
	r.sample(1.0, 3.0);
	BOOST_REQUIRE_EQUAL(r.count(), 3);
	BOOST_REQUIRE_EQUAL(r.correlation(), 0.0);
	BOOST_REQUIRE_EQUAL(r.beta(), 0.0);
	BOOST_REQUIRE_CLOSE(r.alpha(), 2.0, 0.000000001);
}

BOOST_AUTO_TEST_SUITE_END()


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2003-2008 Matthias Christian Schabel
// Copyright (C) 2008 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/**
\file

\brief radar_beam_height.cpp

\details
Demonstrate library usage for user test cases suggested by Michael Fawcett.

Output:
@verbatim

//[radar_beam_height_output
radar range        : 300 nmi
earth radius       : 6.37101e+06 m
beam height 1      : 18169.7 m
beam height 2      : 9.81085 nmi
beam height 3      : 18169.7 m
beam height 4      : 9.81085 nmi
beam height approx : 59488.4 ft
beam height approx : 18132.1 m
//]

@endverbatim
**/

#include <iostream>

#include <boost/units/conversion.hpp>
#include <boost/units/io.hpp>
#include <boost/units/pow.hpp>
#include <boost/units/systems/si.hpp>
#include <boost/units/systems/si/prefixes.hpp>

using boost::units::length_dimension;
using boost::units::pow;
using boost::units::root;
using boost::units::quantity;
using boost::units::unit;

//[radar_beam_height_class_snippet_1
namespace nautical {

struct length_base_unit :
    boost::units::base_unit<length_base_unit, length_dimension, 1>
{
    static std::string name()       { return "nautical mile"; }
    static std::string symbol()     { return "nmi"; }
};

typedef boost::units::make_system<length_base_unit>::type system;

/// unit typedefs
typedef unit<length_dimension,system>    length;

static const length mile,miles;

} // namespace nautical

// helper for conversions between nautical length and si length
BOOST_UNITS_DEFINE_CONVERSION_FACTOR(nautical::length_base_unit,
                                     boost::units::si::meter_base_unit,
                                     double, 1.852e3);
//]

//[radar_beam_height_class_snippet_2
namespace imperial {

struct length_base_unit :
    boost::units::base_unit<length_base_unit, length_dimension, 2>
{
    static std::string name()       { return "foot"; }
    static std::string symbol()     { return "ft"; }
};

typedef boost::units::make_system<length_base_unit>::type system;

/// unit typedefs
typedef unit<length_dimension,system>    length;

static const length foot,feet;

} // imperial

// helper for conversions between imperial length and si length
BOOST_UNITS_DEFINE_CONVERSION_FACTOR(imperial::length_base_unit,
                                     boost::units::si::meter_base_unit,
                                     double, 1.0/3.28083989501312);
//]

// radar beam height functions
//[radar_beam_height_function_snippet_1
template<class System,typename T>
constexpr
quantity<unit<boost::units::length_dimension,System>,T>
radar_beam_height(const quantity<unit<length_dimension,System>,T>& radar_range,
                  const quantity<unit<length_dimension,System>,T>& earth_radius,
                  T k = 4.0/3.0)
{
    return quantity<unit<length_dimension,System>,T>
        (pow<2>(radar_range)/(2.0*k*earth_radius));
}
//]

//[radar_beam_height_function_snippet_2
template<class return_type,class System1,class System2,typename T>
constexpr
return_type
radar_beam_height(const quantity<unit<length_dimension,System1>,T>& radar_range,
                  const quantity<unit<length_dimension,System2>,T>& earth_radius,
                  T k = 4.0/3.0)
{
    // need to decide which system to use for calculation
    return pow<2>(static_cast<return_type>(radar_range))
            / (2.0*k*static_cast<return_type>(earth_radius));
}
//]

//[radar_beam_height_function_snippet_3
constexpr
quantity<imperial::length>
radar_beam_height(const quantity<nautical::length>& range)
{
    return quantity<imperial::length>
        (pow<2>(range/(1.23*nautical::miles/root<2>(imperial::feet))));
}
//]

int main(void)
{
    using namespace boost::units;
    using namespace boost::units::si;
    using namespace nautical;

    //[radar_beam_height_snippet_1
    const quantity<nautical::length> radar_range(300.0*miles);
    const quantity<si::length>       earth_radius(6371.0087714*kilo*meters);

    const quantity<si::length>       beam_height_1(radar_beam_height(quantity<si::length>(radar_range),earth_radius));
    const quantity<nautical::length> beam_height_2(radar_beam_height(radar_range,quantity<nautical::length>(earth_radius)));
    const quantity<si::length>       beam_height_3(radar_beam_height< quantity<si::length> >(radar_range,earth_radius));
    const quantity<nautical::length> beam_height_4(radar_beam_height< quantity<nautical::length> >(radar_range,earth_radius));
    //]

    std::cout << "radar range        : " << radar_range << std::endl
              << "earth radius       : " << earth_radius << std::endl
              << "beam height 1      : " << beam_height_1 << std::endl
              << "beam height 2      : " << beam_height_2 << std::endl
              << "beam height 3      : " << beam_height_3 << std::endl
              << "beam height 4      : " << beam_height_4 << std::endl
              << "beam height approx : " << radar_beam_height(radar_range)
              << std::endl
              << "beam height approx : "
              << quantity<si::length>(radar_beam_height(radar_range))
              << std::endl << std::endl;

    return 0;
}


#pragma once

#include <cslibs_indexed_storage/utility/index_sequence.hpp>
#include <boost/integer.hpp>
#include <array>
#include <valarray>

namespace cslibs_indexed_storage
{
namespace operations
{
namespace clustering
{

namespace detail
{

template <typename Seq1, std::size_t Offset, typename Seq2> struct concat_seq;

template <std::size_t ... Is1, std::size_t Offset, std::size_t ... Is2>
struct concat_seq<utility::index_sequence<Is1...>, Offset, utility::index_sequence<Is2...>>
{
    using type = utility::index_sequence<Is1..., (Offset + Is2)...>;
};

template<typename T>
constexpr T pow(T base, std::size_t exp)
{
    return exp == 0 ? T(1) : base * pow(base, exp - 1);
}

template<typename T>
constexpr T get_bit(std::size_t base, std::size_t bit, std::size_t value)
{
    return bit == 0 ? T(value % base) : get_bit<T>(base, bit - 1, value / base);
}

template<typename offset_t, std::size_t... breaks>
constexpr offset_t generate_offset(std::size_t base, std::size_t counter, utility::index_sequence<breaks...>)
{
    using value_type = typename offset_t::value_type;
    return {value_type(get_bit<value_type>(base, breaks, counter) - base / 2)...};
}

template<typename offset_t>
constexpr offset_t generate_offset(std::size_t base, std::size_t counter)
{
    return generate_offset<offset_t>(base, counter, utility::make_index_sequence<std::tuple_size<offset_t>::value>{});
}

template<typename list_t, std::size_t... counter>
constexpr list_t generate_all(std::size_t base, utility::index_sequence<counter...>)
{
    return {generate_offset<typename list_t::value_type>(base, counter)...};
}

template<typename list_t, std::size_t skip>
constexpr list_t generate(std::size_t base, bool skip_self)
{
    return skip_self ?
           generate_all<list_t>(base,
                                typename concat_seq<
                                        typename utility::make_index_sequence<skip>::type,
                                        skip + 1,
                                        typename utility::make_index_sequence<std::tuple_size<list_t>::value - skip>::type
                                >::type{}) :
           generate_all<list_t>(base, utility::make_index_sequence<std::tuple_size<list_t>::value>{});
}

//! \todo can be increased when unsing a log2 approach in make_index_sequence
constexpr std::size_t MAX_STATIC_SIZE = pow(9ul, 5);

}

template<std::size_t dimensions_, std::size_t size_, bool skip_self_ = true>
class GridNeighborhoodStatic
{
public:
    static constexpr std::size_t dimensions = dimensions_;
    static constexpr std::size_t size = size_;
    static constexpr bool skip_self = skip_self_;

    static_assert((size % 2) == 1, "size must be odd");

    static constexpr std::size_t count = detail::pow(size, dimensions) - (skip_self ? 1 : 0);
    static constexpr std::size_t skip_index = count / 2;
    static constexpr std::size_t shift = size / 2;

    static_assert(count <= detail::MAX_STATIC_SIZE, "static mask size exceeded, use GridNeighborhoodDynamic instead");

    using offset_value_t = typename boost::int_max_value_t<size>::fast;
    using offset_t = std::array<offset_value_t, dimensions>;
    using offset_list_t = std::array<offset_t, count>;

    template<typename visitor_t>
    static inline void visit(const visitor_t& visitor)
    {
        static constexpr offset_list_t offsets = detail::generate<offset_list_t, skip_index>(size, skip_self);
        for (const auto& offset : offsets)
            visitor(offset);
    }
};

class GridNeighborhoodDynamic
{
public:
    using offset_value_t = int64_t;
    using offset_t = std::valarray<offset_value_t>;

    size_t get_dimensions() const
    {
        return dimensions_;
    }

    void set_dimensions(size_t dimensions)
    {
        dimensions_ = dimensions;
    }

    size_t get_size() const
    {
        return size_;
    }

    void set_size(size_t size)
    {
        size_ = size;
    }

    bool is_skip_self() const
    {
        return skip_self_;
    }

    void set_skip_self(bool skip_self)
    {
        skip_self_ = skip_self;
    }

    template<typename visitor_t>
    inline void visit(const visitor_t& visitor)
    {
        const std::size_t count = std::pow<long double>(size_, dimensions_) - (skip_self_ ? 1 : 0);
        const std::size_t skip_index = count / 2;
        const std::size_t shift = size_ / 2;

        for (std::size_t i = 0; i <= count; ++i)
        {
            if (skip_self_ && i == skip_index)
                continue;

            offset_t offset(dimensions_);
            std::size_t value = i;
            for (std::size_t dim = 0; dim < dimensions_; ++dim)
            {
                offset[dim] = offset_value_t(offset_value_t(value % size_) - shift);
                value /= size_;
            }

            visitor(offset);
        }
    }

private:
    std::size_t dimensions_;
    std::size_t size_;
    bool skip_self_;
};

}
}
}


/* Boost example/rational.cpp
 * example program of how to use interval< rational<> >
 *
 * Copyright 2002-2003 Guillaume Melquiond, Sylvain Pion
 *
 * Distributed under the Boost Software License, Version 1.0.
 * (See accompanying file LICENSE_1_0.txt or
 * copy at http://www.boost.org/LICENSE_1_0.txt)
 */

// it would have been enough to only include:
//   <boost/numeric/interval.hpp>
// but it's a bit overkill to include processor intrinsics
// and transcendental functions, so we do it by ourselves

#include <boost/numeric/interval/interval.hpp>      // base class
#include <boost/numeric/interval/rounded_arith.hpp> // default arithmetic rounding policy
#include <boost/numeric/interval/checking.hpp>      // default checking policy
#include <boost/numeric/interval/arith.hpp>         // += *= -= etc
#include <boost/numeric/interval/policies.hpp>      // default policy

#include <boost/rational.hpp>
#include <iostream>

typedef boost::rational<int> Rat;
typedef boost::numeric::interval<Rat> Interval;

std::ostream& operator<<(std::ostream& os, const Interval& r) {
  os << "[" << r.lower() << "," << r.upper() << "]";
  return os;
}

int main() {
  Rat p(2, 3), q(3, 4);
  Interval z(4, 5);
  Interval a(p, q);
  a += z;
  z *= q;
  a -= p;
  a /= q;
  std::cout << z << std::endl;
  std::cout << a << std::endl;
}


#include <cmath>
#include <iostream>
#include <stdio.h>
#include <vector>
#include <sys/time.h>
#include <chrono>
#include <sys/resource.h>   // check the memory usage
#include <stdio.h>
#include <thread>
#include <fstream>
#include <sstream>

#include <NTL/RR.h>
#include <NTL/xdouble.h>
#include <NTL/ZZ.h>
#include <NTL/ZZX.h>
#include <NTL/BasicThreadPool.h>

#include "../src/CZZ.h"
#include "../src/Params.h"
#include "../src/PubKey.h"
#include "../src/Scheme.h"
#include "../src/SchemeAlgo.h"
#include "../src/SecKey.h"
#include "../src/TestScheme.h"
#include "../src/TimeUtils.h"
#include "../src/Ring2Utils.h"
#include "../src/StringUtils.h"
#include "../src/EvaluatorUtils.h"

#include "Database.h"
#include "LRtest.h"
#include "HELR.h"

using namespace NTL;
using namespace std;

int main(int Argc, char** Argv) {

    if(Argc != 2){
        cout << "-------------------------------------------------------------" << endl;
        cerr << "Enter the File and degree of approximation \t"  << "(e.g. $test edin.txt 3) \n ";
    }

    char* filename  =  Argv[1];

    dMat  zData;
    dMat* zTest = new dMat[5];
    dMat* zTrain = new dMat[5];

    int nLine= readData(zData, filename);

    cout << "Sample the learning and test data ..." << endl;
    cvRandomSamplingData(zTrain, zTest, zData, filename);

    //----------------------------------------------------------------
    // Parameters for Logistic regression
    //----------------------------------------------------------------

    long logN= 11;
    long logp= 28;
    long logl= 10;
    long logq, cBit1, cBit2;
    int max_iter;
    long dim ;

    dMat mtheta3_list;
    dMat mtheta7_list;
    dMat mtheta_sig_list;

    ofstream fout;
    fout.open("beta.txt");

    for(int k = 0; k < 5; ++k){

        cout << "-------------------------------------------------------------" << endl;
        cout << "HELR_3 " << endl;

        int polydeg = 3;  // degree of approximation polynomial

        struct LRpar LRparams;
        ReadLRparams(LRparams, max_iter, zTrain[0], polydeg, logp);

        dVec mtheta3(LRparams.dim1, 0.0);
        for(int i= 0; i< LRparams.max_iter; i++){
            LR_poly(mtheta3, zTrain[k], LRparams);
        }

        for(int i= 0; i< LRparams.dim1; i++)
            cout << "[" << mtheta3[i] << "] " ;
        cout  << endl;

        getAUC(mtheta3, zTest[k]);
        mtheta3_list.push_back(mtheta3);

        cout << "-------------------------------------------------------------" << endl;
        cout << "HELR_7 " << endl;

        polydeg = 7;  // degree of approximation polynomial

        struct LRpar LRparams7;
        ReadLRparams(LRparams7, max_iter, zTrain[0], polydeg, logp);

        dVec mtheta7(LRparams7.dim1, 0.0);
        for(int i= 0; i< LRparams7.max_iter; i++){
            LR_poly(mtheta7, zTrain[k], LRparams7);
        }

        for(int i= 0; i< LRparams7.dim1; i++)
            cout << "[" << mtheta7[i] << "] " ;
        cout  << endl;

        getAUC(mtheta7, zTest[k]);
        mtheta7_list.push_back(mtheta7);

        dim = LRparams7.dim1;

        cout << "-------------------------------------------------------------" << endl;
        cout << "sigmoid LR " << endl;
        dVec mtheta_sig(LRparams.dim1, 0.0);

        for(int i= 0; i< LRparams.max_iter; i++){
            LR_sigmoid(mtheta_sig, zTrain[k], LRparams);
        }

        for(int i= 0; i< LRparams.dim1; i++)
            cout << "[" << mtheta_sig[i] << "] " ;
        cout  << endl;

        getAUC(mtheta_sig, zTest[k]);
        mtheta_sig_list.push_back(mtheta_sig);

        cout << "-------------------------------------------------------------" << endl;
        cout << "MSE (HELR/non-HELR): " << getMSE(mtheta3, mtheta_sig) << endl;
        cout << "MSE (HELR/non-HELR): " << getMSE(mtheta7, mtheta_sig) << endl;

    }

    //! write the beta results in the text file
    //fout << "-------------------------------------------------------------" << endl;
    //fout << "HELR_3" << endl;
    for(int i = 0; i < dim; ++i){
        for(int k = 0; k < 4; ++k){
            fout << mtheta3_list[k][i] << "," ;
        }
        fout << mtheta3_list[4][i] << ";" << endl;
    }

    //fout << "-------------------------------------------------------------" << endl;
    //fout << "HELR_7" << endl;
    for(int i = 0; i < dim; ++i){
        for(int k = 0; k < 4; ++k){
            fout << mtheta7_list[k][i] << "," ;
        }
        fout << mtheta7_list[4][i] << ";" << endl;
    }

    //fout << "-------------------------------------------------------------" << endl;
    //fout << "LR" << endl;
    for(int i = 0; i < dim; ++i){
        for(int k = 0; k < 4; ++k){
            fout << mtheta_sig_list[k][i] << "," ;
        }
        fout << mtheta_sig_list[4][i] << ";" << endl;
    }

    fout.close();

    delete[] zTest;
    delete[] zTrain;

    return 0;
}


/*
 * StereoCalib.cpp
 *
 *  Created on: 3 Jan 2015
 *      Author: link
 */

#include <iostream>
#include <list>

#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <StorageFunctions.hpp>
#include <boost/program_options.hpp>

#include "ConvenienceFunctions.hpp"

const int USER_TRIGGERED_EXIT = 0;

void parseCommandline(const int& argc,
                      char** argv,
                      std::string& input,
                      std::string& output)
{

  boost::program_options::options_description desc;

  desc.add_options()("help,h", "this help message")(
    "input,i",
    boost::program_options::value<std::string>(),
    "Input directory containing imageList.xml")(
    "output,o",
    boost::program_options::value<std::string>(),
    "Output file with calibration values for cameras")(
    "length,l",
    boost::program_options::value<double>(),
    "Legth of side of a single chessboard field in chosen units."
    "These units will be used later as measure unit for all calculations");
  boost::program_options::variables_map vm;
  boost::program_options::store(
    boost::program_options::parse_command_line(argc, argv, desc), vm);
  boost::program_options::notify(vm);

  if (vm.count("help"))
  {
    std::cout << desc << std::endl;
    throw USER_TRIGGERED_EXIT;
  }

  if (vm.count("output"))
  {
    output = vm["output"].as<std::string>();
  }

  if (vm.count("input"))
  {
    input = vm["input"].as<std::string>();
  }
}

bool getCorners(const cv::Mat& image,
                const cv::Size& chessboardSize,
                std::vector<cv::Point2f>& corners)
{

  if (cv::findChessboardCorners(image, chessboardSize, corners, 0))
  {
    cv::cornerSubPix(
      image,
      corners,
      cv::Size(11, 11),
      cv::Size(-1, -1),
      cv::TermCriteria(
        cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
    return true;
  }
  else
  {
    return false;
  }
}

int main(int argc, char** argv)
{
  std::string input, output;
  double sideLength;

  parseCommandline(argc, argv, input, output);

  cv::Size imageSize, chessboardSize;
  std::list<std::pair<cv::Mat, cv::Mat>> imageList;

  loadImageList(input, imageSize, chessboardSize, sideLength, imageList);

  std::vector<std::vector<cv::Point3f>> modelPoints(1);

  for (int i = 0; i < chessboardSize.height; ++i)
  {
    for (int j = 0; j < chessboardSize.width; ++j)
    {
      modelPoints[0].push_back(sideLength * cv::Point3f(j, i, 0));
    }
  }

  std::vector<std::vector<cv::Point2f>> leftPoints, rightPoints;
  {
    int i = 0;
    for (auto it = imageList.begin(); imageList.end() != it; ++it, ++i)
    {
      std::pair<cv::Mat, cv::Mat> pair = *it;
      std::cerr << i << std::endl;
      std::vector<cv::Point2f> lP, rP;
      if (getCorners(pair.first, chessboardSize, lP) &&
          getCorners(pair.second, chessboardSize, rP))
      {
        leftPoints.push_back(lP);
        rightPoints.push_back(rP);
      }
      else
      {
        it = imageList.erase(it);
        continue;
      }
    }
  }

  modelPoints.resize(leftPoints.size(), modelPoints[0]);

  cv::Mat lCM, rCM, lDC, rDC, R, T, E, F;

  std::vector<cv::Mat> rvecs, tvecs;

  double leftReprojectionError = cv::calibrateCamera(
    modelPoints, leftPoints, imageSize, lCM, lDC, rvecs, tvecs);
  rvecs.clear();
  tvecs.clear();
  double rightReprojectionError = cv::calibrateCamera(
    modelPoints, rightPoints, imageSize, rCM, rDC, rvecs, tvecs);

  std::cerr << "left rms" << leftReprojectionError << std::endl;
  std::cerr << "right rms" << rightReprojectionError << std::endl;

  double reprojectionError = cv::stereoCalibrate(
    modelPoints,
    leftPoints,
    rightPoints,
    lCM,
    lDC,
    rCM,
    rDC,
    imageSize,
    R,
    T,
    E,
    F,
    cv::CALIB_FIX_INTRINSIC,
    cv::TermCriteria(
      cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, 1e-6));
  std::cerr << reprojectionError << std::endl;

  saveCalibParameters(output,
                      lCM,
                      rCM,
                      lDC,
                      rDC,
                      R,
                      T,
                      E,
                      F,
                      chessboardSize,
                      imageSize,
                      sideLength);
}