/*%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Solving 2D Poisson + Drift Diffusion semiconductor eqns for a solar cell using
%                      Scharfetter-Gummel discretization
%
%                         Written by Timofey Golubev
%
%     This includes the 2D poisson equation and 2D continuity/drift-diffusion
%     equations using Scharfetter-Gummel discretization. The Poisson equation
%     is solved first, and the solution of potential is used to calculate the
%     Bernoulli functions and solve the continuity eqn's.
%
%   Boundary conditions for Poisson equation are:
%
%     -a fixed voltage at (x,0) and (x, Nz) defined by V_bottomBC
%      and V_topBC which are defining the  electrodes
%
%    -insulating boundary conditions: V(0,z) = V(1,z) and
%     V(0,N+1) = V(1,N) (N is the last INTERIOR mesh point).
%     so the potential at the boundary is assumed to be the same as just inside
%     the boundary. Gradient of potential normal to these boundaries is 0.
%
%   Matrix equations are AV*V = bV, Ap*p = bp, and An*n = bn where AV, Ap, and An are sparse matrices
%   (generated using spdiag), for the Poisson and continuity equations.
%   V is the solution for electric potential, p is the solution for hole
%   density, n is solution for electron density
%   bV is the rhs of Poisson eqn which contains the charge densities and boundary conditions
%   bp is the rhs of hole continuity eqn which contains net generation rate
%   and BCs
%
%     The code as is will calculate data for a JV curve
%     as well as carrier densities, current densities, and electric field
%     distributions of a generic solar cell made of an active layer and electrodes.
%     More equations for carrier recombination can be easily added.f
%
%     Photogeneration rate will be inputed from gen_rate.inp file
%     (i.e. the output of an optical model can be used) or an analytic expression
%     for photogeneration rate can be added to photogeneration.cpp. Generation rate file
%     should contain num_cell-2 number of entries in a single column, corresponding to
%     the the generation rate at each mesh point (except the endpoints).
%
%     The code can also be applied to non-illuminated devices by
%     setting photogeneration rate to 0.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%*/

#include <iostream>
#include <vector>
#include <iomanip>
#include <algorithm>   //allows to use fill and min
#include <fstream>
#include <chrono>
#include <string>
#include <time.h>
#include <fstream>
#include <string>

#include <omp.h>

#define EIGEN_USE_MKL_ALL  //is for Intel MKL

//#define EIGEN_NO_DEBUG   //this should turn off eigen asserts, //MAKES NO PERFORMANCE DIFFERENCE!, is I think auto turned off in release mode
#include <Eigen/Sparse>
#include <Eigen/Dense>
#include<Eigen/IterativeLinearSolvers>
#include <Eigen/SparseCholesky>
#include<Eigen/SparseQR>
#include <Eigen/OrderingMethods>
#include<Eigen/SparseLU>
#include <unsupported/Eigen/CXX11/Tensor>  //allows for 3D matrices (Tensors)

#include "constants.h"        //these contain physics constants only
#include "parameters.h"
#include "poisson.h"
#include "continuity_p.h"
//#include "continuity_n.h"
//#include "recombination.h"
//#include "photogeneration.h"
#include "Utilities.h"

#include "mkl.h"

int main()
{

    //trivial MKL function call for testing
    vcAbs(0, 0, 0);  //MY MKL linking works!!, b/c otherwise it wouldn't recognize this function!

    std::chrono::high_resolution_clock::time_point start = std::chrono::high_resolution_clock::now();  //start clock timer
    Parameters params;    //params is struct storing all parameters
    params.Initialize();  //reads parameters from file

    const int num_cell_x = params.num_cell_x;   //create a local num_cell so don't have to type params.num_cell everywhere
    const int num_cell_y = params.num_cell_y;
    const int num_cell_z = params.num_cell_z;

    const int num_V = static_cast<int>(floor((params.Va_max-params.Va_min)/params.increment))+1;  //floor returns double, explicitely cast to int
    params.tolerance_eq = params.tolerance_i;
    const int Nx = params.Nx;
    const int Ny = params.Ny;
    const int Nz = params.Nz;
    const int num_rows = (Nx+1)*(Ny+1)*(Nz+1);  //number of rows in the solution vectors (V, n, p)
    //NOTE: num_rows is the same as num_elements.
    //NOTE: we include the top BC inside the matrix and solution vectors to allow in future to use mixed BC's there.

    std::ofstream JV;
    JV.open("JV.txt");  //note: file will be created inside the build directory

    //-------------------------------------------------------------------------------------------------------
    //Initialize other vectors
    //WILL INDEX FROM 0, b/c that's what Eigen library does.
    //Note: these are Tensor type, so can use reshape on them, but these all are just a single column (i.e. the soln column from the matrix eqn).
    Eigen::Tensor<double, 3> p(num_rows, 1, 1), oldp(num_rows, 1, 1), newp(num_rows, 1, 1);
    Eigen::Tensor<double, 3> oldV(num_rows, 1, 1), newV(num_rows, 1, 1), V(num_rows, 1, 1);

    //create matrices to hold the V, n, and p values (including those at the boundaries) according to the (x,z) coordinates.
    //allows to write formulas in terms of coordinates
    Eigen::VectorXd soln_V(num_rows), soln_p(num_rows);  //vector for storing solutions to the  sparse solver (indexed from 0)
    Eigen::VectorXd p_Xd(num_rows), V_Xd(num_rows);  //Eigen Vector_Xds for the initial buesses to bicgstab method
    //For the following, only need gen rate on insides, so N+1 size is enough
    std::vector<double> Up(num_rows); //will store generation rate as vector, for easy use in rhs

    //Eigen::Tensor<double, 3> R_Langevin(N+1,N+1,N+1), PhotogenRate(N+1,N+1,N+1);
    Eigen::Tensor<double, 3> J_total_Z(num_cell_x+1, num_cell_y+1, num_cell_z+1), J_total_X(num_cell_x+1, num_cell_y+1, num_cell_z+1), J_total_Y(num_cell_x+1, num_cell_y+1, num_cell_z+1);  //we want the indices of J to correspond to the real x,y,z values..., for convinience                //matrices for spacially dependent current
    Eigen::Tensor<double, 3> V_matrix(num_cell_x, num_cell_y, num_cell_z), temp_permuted; //indexed from 0, so just num_cell....//temp_permuted is needed b/c it can't do in place permutations...., need to save to another tensor!  //Note: is actually a Tensor in Eigen
    Eigen::Tensor<double, 3> fullV(num_cell_x+1, num_cell_y+1, num_cell_z+1), fullp(num_cell_x+1, num_cell_y+1, num_cell_z+1);  //for storing all the values in device, including bndrys
    Eigen::SparseMatrix<double> input; //for feeding input matrix into BiCGSTAB, b/c it crashes if try to call get matrix from the solve call.

    //test if openmp is working
    //omp_set_num_threads(8);   //this can allow to set the number of threads that will be used

    std::cout << Eigen::nbThreads() << std::endl;  //displays the # of threads that will be used by Eigen--> mine displays 8, but doesn't seem like it's using 8.

    //this will only make sense if within a region of code which is multithreaded
//    int nthreads = omp_get_num_threads();  //SAYS ONLY 1 THREAD IS AVAILABLE--> MIGHT HAVE AN ISSUE HERE!
//    std::cout << nthreads << " treads available " << std::endl;
//------------------------------------------------------------------------------------
    //Construct objects
    Poisson poisson(params);  //so it can't construct the poisson object
    //Recombo recombo(params);
    Continuity_p continuity_p(params);  //note this also sets up the constant top and bottom electrode BC's

    //Continuity_n continuity_n(params);  //note this also sets up the constant top and bottom electrode BC's
    //Photogeneration photogen(params, params.Photogen_scaling, params.GenRateFileName);
    Utilities utils;
    Eigen::SimplicialLDLT<Eigen::SparseMatrix<double>, Eigen::UpLoType::Lower, Eigen::AMDOrdering<int>> SCholesky; //Note using NaturalOrdering is much much slower

    Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> SQR;
    Eigen::SparseLU<Eigen::SparseMatrix<double> >  poisson_LU, cont_n_LU, cont_p_LU;
    //Eigen::BiCGSTAB<Eigen::SparseMatrix<double>, Eigen::IncompleteLUT<double>> BiCGStab_solver;  //BiCGStab solver object/ /USING THIS PRECONDITIONER IS WAY TOO SLOW FOR LARGE SYSTEMS!
    Eigen::BiCGSTAB<Eigen::SparseMatrix<double>, Eigen::DiagonalPreconditioner<double>> BiCGStab_solver;   //NOTE: WORKS MUCH FASTER WITH DIAGONAL PRECONDITIONER, THAN the IncompleteLUT preconditioner!!--> probably b/c
    //Eigen::BiCGSTAB<Eigen::SparseMatrix<double>, Eigen::IdentityPreconditioner> BiCGStab_solver;  //try with Identity preconditioner, the simplest trivial one
    BiCGStab_solver.setTolerance(1e-14); //set the tolerance explicitely, so matches Matlab's tolerance

    Eigen::ConjugateGradient<Eigen::SparseMatrix<double>, Eigen::UpLoType::Lower|Eigen::UpLoType::Upper > cg;

//--------------------------------------------------------------------------------------------
    //Define boundary conditions and initial conditions. Note: electrodes are at the top and bottom.
    double Va = params.Va_min;
    poisson.set_V_bottomBC(params, Va);
    poisson.set_V_topBC(params, Va);  //THESE CAN BE MOVED TO BE DONE WITHIN constructor of Poisson object

    //Initial conditions
    //for now assume diff is constant everywhere...
    double diff = (poisson.get_V_topBC(0,0) - poisson.get_V_bottomBC(0,0))/num_cell_z;  //this is  calculated correctly

    //Note: B/C I must fill from 0, and don't want to include bottomBC in V matrix, b/c is not part of V
    for (int k = 1; k <= Nz+1; k++) {
        for (int i = 1; i <= Nx+1; i++) {
            for (int j = 1; j <= Ny+1; j++) {
                V_matrix(i-1, j-1, k-1) = poisson.get_V_bottomBC(i,j) +  diff*(k);  //-1's b/c fill from 0
            }
        }
    }

    //need to permute the matrix, to be consistent with the z,y,x ordering which I use for the matrices when solving.
    //NOTE: for Tensor shuffle to work, NEED TO EXPLICTELY CREATE AN ARRAY--> this isn't clear from the documentation
    Eigen::array<ptrdiff_t, 3> permutation = {{2,1,0}}; //array should HAVE THE SPECIFIC type:  ptrdiff_t  ==> used for pointer arithmetic and array indexing.
    //ptrdiff_t is the signed integer type of the result of subtracting 2 pointers.

    temp_permuted = V_matrix.shuffle(permutation);  //shuffle permuts the tensor. Note: dimensions are indexed from 0. This is supposed to swap x and z values...
    V_matrix = temp_permuted;
    //Returns a copy of the input tensor whose dimensions have been reordered according to the specified permutation. The argument shuffle is an array of Index values. Its size is the rank of the input tensor. It must contain a permutation of 0, 1, ..., rank - 1.
    //works to here
    //NOTE: IT CAN'T DO AN INPLACE PERMUTATION!!!--> need to rename the variable!!!

    //reshape the matrix to a single column. //Note: even though reshaping to a column, V still must be a TENSOR type for this to work!
     Eigen::array<ptrdiff_t, 3> reshape_sizes = {{num_rows, 1, 1}};  //need to only go to num_rows..., b/c it fills from 0 !!
     V = V_matrix.reshape(reshape_sizes);  //Note: even though reshaping to a column, V still must be a TENSOR type for this to work!

     //prepare initial guess, for 1st iteration of bicgstab
     for (int i = 0; i < num_rows; i++) {
         V_Xd(i) = V(i,0,0);
         soln_V(i) = V(i,0,0); // do this since we are using soln_V for the inital guess
     }

    //Fill p with initial conditions (need for error calculation)
    double min_dense = continuity_p.get_p_bottomBC(1,1) < continuity_p.get_p_topBC(1,1) ? continuity_p.get_p_bottomBC(1,1):continuity_p.get_p_topBC(1,1);  //this should be same as std::min  fnc which doesn't work for some reason
    //double min_dense = std::min (continuity_n.get_n_bottomBC(1,1), continuity_p.get_p_topBC(1,1));  //Note: I defined the get fnc to take as arguments the i,j values... //Note: the bc's along bottom and top are currently uniform, so index, doesn't really matter.
    for (int i = 0; i < num_rows; i++) {
        p(i,0,0) = min_dense;  //NOTE: p is tensor now, so need () to access elements
    }

    //prepare initial guess, for 1st iteration of bicgstab
    for (int i = 0; i < num_rows; i++) {
        p_Xd(i) = p(i,0,0);  //this is working correctly
        soln_p(i) = p(i,0,0); // do this since we are using soln_p for the inital guess
    }

    //Convert the p to p_matrix
    Eigen::array<ptrdiff_t, 3> to_matrix_sizes{{Nz+1, Ny+1, Nx+1}};  //use reshape according to the  ordering of the matrix, so: z, y, x
    Eigen::Tensor<double, 3> p_matrix = p.reshape(to_matrix_sizes);
    //continuity_p.set_p_matrix(p_matrix);  //THIS FOR SOME REASON FAILS..., so don't use it...//save p_matrix to continuity_p member variable--> THIS MIGHT BE INEFFIIENT, BUT DO IT FOR NOW

    //////////////////////MAIN LOOP////////////////////////////////////////////////////////////////////////////////////////////////////////

    int iter, not_cnv_cnt, Va_cnt;
    bool not_converged;
    double error_np, old_error;  //this stores max value of the error and the value of max error from previous iteration
    std::vector<double> error_np_vector(num_rows);  //note: since n and p solutions are in vector form, can use vector form here also

    poisson.setup_matrix();  //I VERIFIED that size of sparse matrix is correct

    for (Va_cnt = 1; Va_cnt <= num_V; Va_cnt++) {  //+1 b/c 1st Va is the equil run
        not_converged = false;
        not_cnv_cnt = 0;

        Va = params.Va_min+params.increment*(Va_cnt-1);

        if (Va_cnt == 1) {
            params.use_tolerance_i();  //reset tolerance back
            params.use_w_i();
            //PhotogenRate = photogen.getPhotogenRate();    //otherwise PhotogenRate is pre-initialized to 0 in this main.cpp when declared
        }

        if (params.tolerance > 1e-5)
            std::cerr<<"ERROR: Tolerance has been increased to > 1e-5" <<std::endl;

        std::cout << "Va = " << Va <<std::endl;

        //Reset top and bottom BCs (outside of loop b/c don't change iter to iter)
        poisson.set_V_bottomBC(params, Va);
        poisson.set_V_topBC(params, Va);

       //correct through  here

        //-----------------------------------------------------------
        error_np = 1.0;
        iter = 0;

        //get's through here
        while (error_np > params.tolerance) {
            //std::cout << "Va " << Va <<std::endl;

            //-----------------Solve Poisson Equation------------------------------------------------------------------
            poisson.set_rhs(p);  //this finds netcharge and sets rhs
            //std::cout << poisson.get_sp_matrix() << std::endl;
            oldV = V;

            input = poisson.get_sp_matrix();

            //as expected, LU, is way too slow for a 3D matrix!!

            if (iter == 0) {
                BiCGStab_solver.analyzePattern(input);
                BiCGStab_solver.factorize(input);  //this computes preconditioner, Poisson matrix doesn't change, so can factorize just once
            }
            //BiCGStab_solver.compute(input);  //this computes the preconditioner..compute(input);
            //soln_V = BiCGStab_solver.solve(poisson.get_rhs());
            soln_V = BiCGStab_solver.solveWithGuess(poisson.get_rhs(), soln_V); //note: using soln_V for initial guess is faster than using V_Xd/NOTE: use solve with Guess...., b/c need initial guess
            //std::cout << "#iterations:     " << BiCGStab_solver.iterations() << std::endl;
             //std::cout << BiCGStab_solver.info() << std::endl;
            //std::cout << soln_V << std::endl;

           //CHOLESKY is not accurate!! for 3D solve

            //std::cout << poisson.get_sp_matrix() << std::endl;
             //std::cout << "Poisson solver error " << poisson.get_sp_matrix() * soln_Xd - poisson.get_rhs() << std::endl;

            //RECALL, I am starting my V vector from index of 1, corresponds to interior pts...
            for (int i = 0; i < num_rows; i++) {
                newV(i,0,0) = soln_V(i);   //fill VectorXd  rhs of the equation
                //NOTE: newV is a Tensor now, so use () to access
            }

            //Mix old and new solutions for V
            if (iter > 0)
                V  = utils.linear_mix(params, newV, oldV);
            else
                V = newV;

            //update the Eigen Vector_Xd for the initial guess--> LATER DO ALL THIS MORE EFFICIENTLY
            for (int i = 0; i < num_rows; i++) {
                V_Xd(i) = V(i,0,0);
            }

            //reshape solution to a V_matrix
            V_matrix = V.reshape(to_matrix_sizes); // reshapes using z, y, x ordering
            //need to permute before using for fullV
            temp_permuted = V_matrix.shuffle(permutation);  //need to use a temp variable for this to work

            //fill the fullV
            //SHOULD MOVE THIS TO A FUNCTION!
            for (int k = 1; k <= num_cell_z; k++)
               for (int j = 1; j <= num_cell_y; j++)
                   for (int i = 1; i <= num_cell_x; i++)
                       fullV(i,j,k) = temp_permuted(i-1,j-1,k-1);  //-1 b/c temp_permuted from V_matrix was filled from 0

            for (int j = 0; j <= num_cell_y; j++)
                for (int i = 0; i <= num_cell_x; i++)
                    fullV(i,j,0) = poisson.get_V_bottomBC(i,j);

            for (int k = 1; k <= num_cell_z; k++)
               for (int j = 1; j <= num_cell_y; j++)
                   fullV(0,j,k) = temp_permuted(num_cell_x-1,j-1,k-1);  //x BC's

            for (int k = 1; k <= num_cell_z; k++)
               for (int i = 1; i <= num_cell_x; i++)
                   fullV(i,0,k) = temp_permuted(i-1,num_cell_y-1,k-1);  //y BC's

            //fill edges
            for (int k = 1; k <= num_cell_z; k++)
                fullV(0,0,k) = temp_permuted(num_cell_x-1,0,k-1);

            //------------------------------Calculate Net Generation Rate----------------------------------------------------------

            //R_Langevin = recombo.ComputeR_Langevin(params,n,p);
            //FOR NOW CAN USE 0 FOR R_Langevin

                for (int i = 0; i < num_rows; i++) {
                    Up[i] = 0; //params.Photogen_scaling;  //This is what was used in Matlab version for testing.   photogen.getPhotogenRate()(i,j); //- R_Langevin(i,j);
                }

            //--------------------------------Solve equation for p------------------------------------------------------------

            continuity_p.setup_eqn(fullV, Up, p);  //pass it fullV...
            //std::cout << continuity_p.get_sp_matrix() << std::endl;   //Note: get rhs, returns an Eigen VectorXd

            for (int i = 0; i < num_rows; i++)
                oldp(i,0,0) = p(i,0,0);          //explicitely  copy, just in case

            input = continuity_p.get_sp_matrix();
            if (iter == 0) {
                BiCGStab_solver.analyzePattern(input);
                BiCGStab_solver.factorize(input);  //factorize only for the 1st iteration!!, since matrix doesn't change too much, this still will work!-> means it only  computes preconditioner once per Va!! //this computes preconditioner, if use along with analyzePattern (for 1st iter)
            }
            //BiCGStab_solver.compute(input);  //this computes the preconditioner..compute(input);
            soln_p = BiCGStab_solver.solveWithGuess(continuity_p.get_rhs(), soln_p);  //NOTE: if for initial guess use soln_p, INSTEAD OF p_Xd (which is the linearly mixed solution, then does't blow up!!!!!, even with 0.2 = w.
            //soln_p = BiCGStab_solver.solve(continuity_p.get_rhs());

//         std::cout << soln_p << std::endl;
//         exit(1);

            //save results back into n std::vector. RECALL, I am starting my V vector from index of 1, corresponds to interior pts...
            for (int i = 0; i < num_rows; i++) {
                newp(i,0,0) = soln_p(i);   //newp is now a tensor....
            }

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

            //if get negative p's or n's set them = 0
            for (int i = 0; i < num_rows; i++) {
                if (newp(i,0,0) < 0.0) newp(i,0,0) = 0;
                //if (newn[i] < 0.0) newn[i] = 0;
            }

            //calculate the error
            old_error = error_np;
            int count = 0;  //for counting the error_np_vector_index

            //THIS CAN BE MOVED TO A FUNCTION IN UTILS
            std::fill(error_np_vector.begin(), error_np_vector.end(),0.0);  //refill with 0's so have fresh one
            for (int i = 0; i < num_rows; i++) {
                if (newp(i,0,0)!= 0) {
                    error_np_vector[count] = (std::abs(newp(i,0,0)-oldp(i,0,0)))/std::abs(oldp(i,0,0));
                    count++;
//                   if(iter == 2)
//                       std::cout << error_np_vector[i] << std::endl;
               }
            }

//            if(iter == 2) {
//                for (int i = 0; i < num_rows; i++ ) {
//                    std::cout <<  newp(i,0,0) << " " << oldp(i,0,0) <<  std::endl;   //FOR LARGE SYSTEMS SEEMS NEWP AND OLDP ARE EXACT SAME--> GETTING 0 ERRORS, BUT THEN BLOWS UP!!  //compare newp and oldp //newp are not 0's --> have numbers...
//                }
//                exit(1);
//            }

            error_np = *std::max_element(error_np_vector.begin(),error_np_vector.end());

            std::cout << error_np << std::endl;

            //auto decrease w if not converging
            if (error_np >= old_error)
                not_cnv_cnt = not_cnv_cnt+1;
            if (not_cnv_cnt > 1000) {  //Note: 100 is too small for C++, sometimes w is reduced when not necessary!!
                params.reduce_w();
                params.relax_tolerance();
                not_cnv_cnt = 0;
            }

            p = utils.linear_mix(params, newp, oldp);

            //update the Eig vector for initial guess --> LATER SHOULD DO ALL OF THIS MORE EFFICIENTLY
            for (int i = 0; i < num_rows; i++) {
                p_Xd(i) = p(i,0,0);
            }

            //Convert p to p_matrix
            p_matrix = p.reshape(to_matrix_sizes);  // this reshapes based on z,y,x ordering

            //need to permute before using for fullV
            temp_permuted = p_matrix.shuffle(permutation);  //need to use a temp variable for this to work

            //fill the fullp
            //SHOULD MOVE THIS TO A FUNCTION!
            for (int k = 1; k <= num_cell_z; k++)
               for (int j = 1; j <= num_cell_y; j++)
                   for (int i = 1; i <= num_cell_x; i++)
                       fullp(i,j,k) = temp_permuted(i-1,j-1,k-1);  //-1 b/c temp_permuted from V_matrix was filled from 0

            for (int j = 0; j <= num_cell_y; j++)
                for (int i = 0; i <= num_cell_x; i++)
                    fullp(i,j,0) = continuity_p.get_p_bottomBC(i,j);

            for (int k = 1; k <= num_cell_z; k++)
               for (int j = 1; j <= num_cell_y; j++)
                   fullp(0,j,k) = temp_permuted(num_cell_x-1,j-1,k-1);  //x BC's

            for (int k = 1; k <= num_cell_z; k++)
               for (int i = 1; i <= num_cell_x; i++)
                   fullp(i,0,k) = temp_permuted(i-1,num_cell_y-1,k-1);  //y BC's

            //fill edges
            for (int k = 1; k <= num_cell_z; k++)
                fullp(0,0,k) = temp_permuted(num_cell_x-1,0,k-1);

            //continuity_p.set_p_matrix(p_matrix);  //update member variable  //DON'T USE THIS B/C CAUSES ISSUES, just use the p matrix form main

           // std::cout << error_np << std::endl;
            //std::cout << "weighting factor = " << params.w << std::endl << std::endl;

            iter = iter+1;
        }

        //-------------------Calculate Currents using Scharfetter-Gummel definition--------------------------

        //continuity_n.calculate_currents();
        continuity_p.calculate_currents(fullp); //send it fullp, since we find it here anyway

        J_total_Z = continuity_p.get_Jp_Z();// + continuity_n.get_Jn_Z();
        J_total_X = continuity_p.get_Jp_X();// + continuity_n.get_Jn_X();
        J_total_Y = continuity_p.get_Jp_Y();// + continuity_n.get_Jn_Y();

//        for (int k = 1; k < num_cell_z; k++)
//            std::cout << J_total_Z(2,2,k) << std::endl;
//exit(1);

        //---------------------Write to file----------------------------------------------------------------
        utils.write_details(params, Va, fullV, fullp, J_total_Z, Up);

        if(Va_cnt >0) utils.write_JV(params, JV, iter, Va, J_total_Z);

    }//end of main loop

    JV.close();

    std::chrono::high_resolution_clock::time_point finish = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> time = std::chrono::duration_cast<std::chrono::duration<double>>(finish-start);
    std::cout << "CPU time = " << time.count() << std::endl;

    return 0;
}


/*
 * @author Jenna Reher (jreher@caltech.edu)
 */

#ifndef CASSIE_GEOMETRY_HPP
#define CASSIE_GEOMETRY_HPP

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <unsupported/Eigen/EulerAngles>

/* FUNCTION eulerZYX(float q[4], float euler[3])
 * Convert the quaternion to euler angles in ZYX rotation order.
 */
void eulerZYX(Eigen::Quaterniond &q, Eigen::EulerAnglesZYXd &euler);

void eulerXYZ(Eigen::Quaterniond &q, Eigen::EulerAnglesXYZd &euler);

void eulerXYZ(Eigen::Matrix3d &R, Eigen::EulerAnglesXYZd &euler);

Eigen::Matrix3d skew(Eigen::Vector3d &v);

#endif // CASSIE_GEOMETRY_HPP


#include <Eigen/Geometry>

#include "recalibration.h"

void Recalibration::translate (std::vector<float>& vertices, const QVector3D &p) {

    for (int i = 0; i < vertices.size(); i += 3) {
        vertices[i] += p[0];
        vertices[i+1] += p[1];
        vertices[i+2] += p[2];
    }
}

void Recalibration::rotate (std::vector<float>& vertices, const QVector3D &angle) {

    Eigen::Vector4f vec;
    Eigen::Vector4f vecRot;

    Eigen::Quaternionf q = Eigen::Quaternionf(0, angle.x(), angle.y(), angle.z());
    Eigen::Quaternionf qInv = q.inverse();

    for (int i = 0; i < vertices.size(); i += 3) {

        vec = Eigen::Vector4f(vertices[i], vertices[i+1], vertices[i+2], 0);

        vecRot = q * vec * qInv;

        vertices[i] = vecRot.x();
        vertices[i+1] = vecRot.y();
        vertices[i+2] = vecRot.z();
    }
}

void Recalibration::recalibrate (std::vector<float>& vertices, const QVector3D &v1, const QVector3D &v2, const QVector3D &v3, const QVector3D &v1p, const QVector3D &v2p, const QVector3D &v3p) {

    // Translation
    QVector3D vecTranslation = vectorPointToPoint(v1, v1p);
    translate(vertices, vecTranslation);

    // Rotation
    QVector3D angle1 = angleBetweenVectors(v1p, v2, v1p, v2); // A vérifier
    QVector3D angle2 = angleBetweenVectors(v1p, v3, v1p, v3); // A vérifier

    rotate (vertices, angle1 + angle2);
}

QVector3D Recalibration::vectorPointToPoint(const QVector3D &p1,const  QVector3D &p2) const{

    return QVector3D(p2.x() - p1.x(), p2.y() - p1.y(), p2.z() - p1.z());
}

QVector3D Recalibration::angleBetweenVectors(const QVector3D& ov1, const QVector3D& fv1, const QVector3D& ov2, const QVector3D& fv2) const {

    QVector3D res;

    // Axe X

    QVector3D dirv1 = QVector3D(fv1.x() - fv1.x(), 0, 0);
    QVector3D dirv2 = QVector3D(fv2.x() - fv2.x(), 0, 0);

    float scalar = dirv1.x() * dirv2.x();

    float norm = sqrt(dirv1.x() * dirv1.x()) * sqrt(dirv2.x() * dirv2.x());

    res.setX(acos(scalar / norm));

    // Axe Y

    dirv1 = QVector3D(0, fv1.y() - fv1.y(), 0);
    dirv2 = QVector3D(0, fv2.y() - fv2.y(), 0);

    scalar = dirv1.y() * dirv2.y();

    norm = sqrt(dirv1.y() * dirv1.y()) * sqrt(dirv2.y() * dirv2.y());

    res.setY(acos(scalar / norm));

    // Axe Z

    dirv1 = QVector3D(fv1.z() - fv1.z(), 0, 0);
    dirv2 = QVector3D(fv2.z() - fv2.z(), 0, 0);

    scalar = dirv1.z() * dirv2.z();

    norm = sqrt(dirv1.z() * dirv1.z()) * sqrt(dirv2.z() * dirv2.z());

    res.setZ(acos(scalar / norm));

    return res;

    /*

    QVector3D dirv1 = QVector3D(fv1.x - fv1.x, fv1.y - ov1.y, fv1.z - ov1.z);
    QVector3D dirv2 = QVector3D(fv2.x - fv2.x, fv2.y - ov2.y, fv2.z - ov2.z);

    float scalar = dirv1.x * dirv2.x + irv1.y * dirv2.y + irv1.z * dirv2.z;

    float norm = sqrt(dirv1.x() * dirv1.x() + dirv1.y() * dirv1.y() + dirv1.z() * dirv1.z()) *
                 sqrt(dirv2.x() * dirv2.x() + dirv2.y() * dirv2.y() + dirv2.z() * dirv2.z());

    return acos(scalarP / norm);*/
}


//
// Copyright 2019 Olzhas Zhumabek <anonymous.from.applecity@gmail.com>
//
// Use, modification and distribution are subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
#include <boost/gil/detail/math.hpp>
#include <boost/gil/image_processing/numeric.hpp>

#include <boost/core/lightweight_test.hpp>

#include <algorithm>

#include "test_utility_output_stream.hpp"

namespace gil = boost::gil;

void test_dx_sobel_kernel() {
  auto const kernel = gil::generate_dx_sobel(1);
  BOOST_TEST_ALL_EQ(kernel.begin(), kernel.end(), gil::detail::dx_sobel.begin(),
                    gil::detail::dx_sobel.end());
}

void test_dx_scharr_kernel() {
  auto const kernel = gil::generate_dx_scharr(1);
  BOOST_TEST_ALL_EQ(kernel.begin(), kernel.end(),
                    gil::detail::dx_scharr.begin(),
                    gil::detail::dx_scharr.end());
}

void test_dy_sobel_kernel() {
  auto const kernel = gil::generate_dy_sobel(1);
  BOOST_TEST_ALL_EQ(kernel.begin(), kernel.end(), gil::detail::dy_sobel.begin(),
                    gil::detail::dy_sobel.end());
}

void test_dy_scharr_kernel() {
  auto const kernel = gil::generate_dy_scharr(1);
  BOOST_TEST_ALL_EQ(kernel.begin(), kernel.end(),
                    gil::detail::dy_scharr.begin(),
                    gil::detail::dy_scharr.end());
}

int main() {
  test_dx_sobel_kernel();
  test_dx_scharr_kernel();
  test_dy_sobel_kernel();
  test_dy_scharr_kernel();
  return boost::report_errors();
}


#include "problem_generator.h"
#include <Eigen/Dense>
#include <random>
#include <vector>

namespace pose_lib {

static const double kPI = 3.14159265358979323846;

double CalibPoseValidator::compute_pose_error(const ProblemInstance &instance, const CameraPose &pose) {
  return (instance.pose_gt.R - pose.R).norm() + (instance.pose_gt.t - pose.t).norm() + std::abs(instance.pose_gt.alpha - pose.alpha);
}

bool CalibPoseValidator::is_valid(const ProblemInstance &instance, const CameraPose &pose, double tol) {
  if ((pose.R.transpose() * pose.R - Eigen::Matrix3d::Identity()).norm() > tol)
    return false;

  // Point to point correspondences
  // alpha * p + lambda*x = R*X + t
  for (int i = 0; i < instance.x_point_.size(); ++i) {
    double err = 1.0 - std::abs(instance.x_point_[i].dot((pose.R * instance.X_point_[i] + pose.t - pose.alpha * instance.p_point_[i]).normalized()));
    if (err > tol)
      return false;
  }

  // Point to Line correspondences
  // alpha * p + lambda * x = R*(X + mu*V) + t
  for (int i = 0; i < instance.x_line_.size(); ++i) {
    // lambda * x - mu * R*V = R*X + t - alpha * p
    // x.cross(R*V).dot(R*X+t-alpha.p) = 0
    Eigen::Vector3d X = pose.R * instance.X_line_[i] + pose.t - pose.alpha * instance.p_line_[i];
    double err = instance.x_line_[i].cross(pose.R * instance.V_line_[i]).normalized().dot(X);

    if (err > tol)
      return false;
  }

  // Line to point correspondences
  // l'*(R*X + t - alpha*p) = 0
  for (int i = 0; i < instance.l_line_point_.size(); ++i) {

    Eigen::Vector3d X = pose.R * instance.X_line_point_[i] + pose.t - pose.alpha * instance.p_line_point_[i];

    double err = std::abs(instance.l_line_point_[i].dot(X.normalized()));
    if (err > tol)
      return false;
  }

  // Line to line correspondences
  // l'*(R*(X + mu*V) + t - alpha*p) = 0
  for (int i = 0; i < instance.l_line_line_.size(); ++i) {

    Eigen::Vector3d X = pose.R * instance.X_line_line_[i] + pose.t - pose.alpha * instance.p_line_line_[i];
    Eigen::Vector3d V = pose.R * instance.V_line_line_[i];

    double err = std::abs(instance.l_line_line_[i].dot(X.normalized())) + std::abs(instance.l_line_line_[i].dot(V.normalized()));
    if (err > tol)
      return false;
  }

  return true;
}

double UnknownFocalValidator::compute_pose_error(const ProblemInstance &instance, const CameraPose &pose) {
  return (instance.pose_gt.R - pose.R).norm() + (instance.pose_gt.t - pose.t).norm() + std::abs(instance.pose_gt.alpha - pose.alpha);
}

bool UnknownFocalValidator::is_valid(const ProblemInstance &instance, const CameraPose &pose, double tol) {
  if ((pose.R.transpose() * pose.R - Eigen::Matrix3d::Identity()).norm() > tol)
    return false;

  if (pose.alpha < 0)
    return false;

  Eigen::Matrix3d Kinv;
  Kinv.setIdentity();
  Kinv(2, 2) = pose.alpha;
  // lambda*diag(1,1,alpha)*x = R*X + t
  for (int i = 0; i < instance.x_point_.size(); ++i) {
    double err = 1.0 - std::abs((Kinv * instance.x_point_[i]).normalized().dot((pose.R * instance.X_point_[i] + pose.t).normalized()));
    if (err > tol)
      return false;
  }

  return true;
}

double RadialPoseValidator::compute_pose_error(const ProblemInstance &instance, const CameraPose &pose) {
  // Only compute up to sign for radial cameras

  double err1 = (instance.pose_gt.R.topRows(2) - pose.R.topRows(2)).norm() + (instance.pose_gt.t.topRows(2) - pose.t.topRows(2)).norm();
  double err2 = (instance.pose_gt.R.topRows(2) + pose.R.topRows(2)).norm() + (instance.pose_gt.t.topRows(2) + pose.t.topRows(2)).norm();

  return std::min(err1, err2);
}

bool RadialPoseValidator::is_valid(const ProblemInstance &instance, const CameraPose &pose, double tol) {
  if ((pose.R.transpose() * pose.R - Eigen::Matrix3d::Identity()).norm() > tol)
    return false;

  // Point to point correspondences -- Convert these to line correspondences
  // alpha * p + lambda*x = R*X + t
  for (int i = 0; i < instance.x_point_.size(); ++i) {
    Eigen::Vector3d radial_line{-instance.x_point_[i](1),instance.x_point_[i](0), 0.0};
    Eigen::Vector3d X = pose.R * instance.X_point_[i] + pose.t;
    double err = std::abs(radial_line.dot(X.normalized()));
    if (err > tol)
      return false;
  }

  // Line to point correspondences
  // l'*(R*X + t) = 0
  for (int i = 0; i < instance.l_line_point_.size(); ++i) {
    Eigen::Vector3d X = pose.R * instance.X_line_point_[i] + pose.t;

    double err = std::abs(instance.l_line_point_[i].dot(X.normalized()));
    if (err > tol)
      return false;
  }

  return true;
}

void set_random_pose(CameraPose &pose, bool upright) {
  if (upright) {
    Eigen::Vector2d r;
    r.setRandom().normalize();
    //pose.R << r(0), 0.0, r(1), 0.0, 1.0, 0.0, -r(1), 0.0, r(0); // y-gravity
    pose.R << r(0), r(1), 0.0, -r(1), r(0), 0.0, 0.0, 0.0, 1.0; // z-gravity
  } else {
    pose.R = Eigen::Quaternion<double>::UnitRandom();
  }
  pose.t.setRandom();
}

void generate_problems(int n_problems, std::vector<ProblemInstance> *problem_instances,
                       const ProblemOptions &options) {
  problem_instances->clear();
  problem_instances->reserve(n_problems);

  double fov_scale = std::tan(options.camera_fov_ / 2.0 * kPI / 180.0);

  // Random generators
  std::default_random_engine random_engine;
  std::uniform_real_distribution<double> depth_gen(options.min_depth_, options.max_depth_);
  std::uniform_real_distribution<double> coord_gen(-fov_scale, fov_scale);
  std::uniform_real_distribution<double> scale_gen(options.min_scale_, options.max_scale_);
  std::uniform_real_distribution<double> focal_gen(options.min_focal_, options.max_focal_);
  std::normal_distribution<double> direction_gen(0.0, 1.0);
  std::normal_distribution<double> offset_gen(0.0, 1.0);

  for (int i = 0; i < n_problems; ++i) {
    ProblemInstance instance;
    set_random_pose(instance.pose_gt, options.upright_);

    if (options.unknown_scale_) {
      instance.pose_gt.alpha = scale_gen(random_engine);
    } else if (options.unknown_focal_) {
      instance.pose_gt.alpha = focal_gen(random_engine);
    }

    // Point to point correspondences
    instance.x_point_.reserve(options.n_point_point_);
    instance.X_point_.reserve(options.n_point_point_);
    instance.p_point_.reserve(options.n_point_point_);
    for (int j = 0; j < options.n_point_point_; ++j) {

      Eigen::Vector3d p{0.0, 0.0, 0.0};
      Eigen::Vector3d x{coord_gen(random_engine), coord_gen(random_engine), 1.0};
      x.normalize();
      Eigen::Vector3d X;

      if (options.generalized_) {
        p << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine);
      }

      X = instance.pose_gt.alpha * p + x * depth_gen(random_engine);

      X = instance.pose_gt.R.transpose() * (X - instance.pose_gt.t);

      if (options.unknown_focal_) {
        x.block<2, 1>(0, 0) *= instance.pose_gt.alpha;
        x.normalize();
      }

      instance.x_point_.push_back(x);
      instance.X_point_.push_back(X);
      instance.p_point_.push_back(p);
    }

    // Point to line correspondences
    instance.x_line_.reserve(options.n_point_line_);
    instance.X_line_.reserve(options.n_point_line_);
    instance.V_line_.reserve(options.n_point_line_);
    instance.p_line_.reserve(options.n_point_line_);
    for (int j = 0; j < options.n_point_line_; ++j) {
      Eigen::Vector3d p{0.0, 0.0, 0.0};
      Eigen::Vector3d x{coord_gen(random_engine), coord_gen(random_engine), 1.0};
      x.normalize();
      Eigen::Vector3d X;

      if (options.generalized_) {
        p << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine);
      }
      X = instance.pose_gt.alpha * p + x * depth_gen(random_engine);
      X = instance.pose_gt.R.transpose() * (X - instance.pose_gt.t);

      Eigen::Vector3d V{direction_gen(random_engine), direction_gen(random_engine), direction_gen(random_engine)};
      V.normalize();

      // Translate X such that X.dot(V) = 0
      X = X - V.dot(X) * V;

      if (options.unknown_focal_) {
        // TODO implement this.
      }

      instance.x_line_.push_back(x);
      instance.X_line_.push_back(X);
      instance.V_line_.push_back(V);
      instance.p_line_.push_back(p);
    }

    // Line to point correspondences
    instance.l_line_point_.reserve(options.n_line_point_);
    instance.X_line_point_.reserve(options.n_line_point_);
    instance.p_line_point_.reserve(options.n_line_point_);
    for (int j = 0; j < options.n_line_point_; ++j) {
      Eigen::Vector3d p{0.0, 0.0, 0.0};
      Eigen::Vector3d x{coord_gen(random_engine), coord_gen(random_engine), 1.0};
      x.normalize();
      Eigen::Vector3d X;

      if (options.generalized_) {
        p << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine);
      }
      X = instance.pose_gt.alpha * p + x * depth_gen(random_engine);
      X = instance.pose_gt.R.transpose() * (X - instance.pose_gt.t);

      // Cross product with random vector to generate line
      Eigen::Vector3d l;
      if(options.radial_lines_) {
        // Line passing through image center
        l = x.cross(Eigen::Vector3d{0.0, 0.0, 1.0});
      } else {
        // Random line
        l = x.cross(Eigen::Vector3d(direction_gen(random_engine), direction_gen(random_engine), direction_gen(random_engine)));
      }

      l.normalize();

      if (options.unknown_focal_) {
        // TODO implement this.
      }

      instance.l_line_point_.push_back(l);
      instance.X_line_point_.push_back(X);
      instance.p_line_point_.push_back(p);
    }

    // Line to line correspondences
    instance.l_line_line_.reserve(options.n_line_line_);
    instance.X_line_line_.reserve(options.n_line_line_);
    instance.V_line_line_.reserve(options.n_line_line_);
    instance.p_line_line_.reserve(options.n_line_line_);
    for (int j = 0; j < options.n_line_line_; ++j) {
      Eigen::Vector3d p{0.0, 0.0, 0.0};
      Eigen::Vector3d x{coord_gen(random_engine), coord_gen(random_engine), 1.0};
      x.normalize();
      Eigen::Vector3d X;

      if (options.generalized_) {
        p << offset_gen(random_engine), offset_gen(random_engine), offset_gen(random_engine);
      }
      X = instance.pose_gt.alpha * p + x * depth_gen(random_engine);
      X = instance.pose_gt.R.transpose() * (X - instance.pose_gt.t);

      Eigen::Vector3d V{direction_gen(random_engine), direction_gen(random_engine), direction_gen(random_engine)};
      V.normalize();

      // Translate X such that X.dot(V) = 0
      X = X - V.dot(X) * V;

      Eigen::Vector3d l = x.cross(instance.pose_gt.R * V);
      l.normalize();

      if (options.unknown_focal_) {
        // TODO implement this.
      }

      instance.l_line_line_.push_back(l);
      instance.X_line_line_.push_back(X);
      instance.V_line_line_.push_back(V);
      instance.p_line_line_.push_back(p);
    }

    problem_instances->push_back(instance);
  }
}

}; // namespace pose_lib


//
// Created by Xinyu Zhang on 3/26/21.
//
#include <iostream>

// #include <Eigen/Dense>
// #include <cosan/io/utils.h>
#include <cosan/data/CosanData.h>
// #include <cosan/model/CosanLinearRegression.h>
// #include <cosan/model/CosanRidgeRegression.h>
//using namespace Eigen;
//using namespace std;

int main() {
//    Cosan::CosanRawData CD("./example_data/toy/X_.csv","./example_data/toy/y.csv");
    Cosan::CosanRawData<long double> CRD("./example_data/toy2/X_.csv");

    // Cosan::CosanLinearRegression CLR(true);
//     CLR.fit(CD.GetInput(),CD.GetTarget());
    std::cout<<CRD.GetSummaryMessageX()<<std::endl;
    std::cout<<CRD.GetInput()<<std::endl;
//    double RegularizationTerm = 1;
//    Cosan::CosanRidgeRegression CRR(RegularizationTerm,true);
//    CRR.fit(CD.GetInput(),CD.GetTarget());
//    std::cout<<CRR.GetBeta()<<std::endl;
//    save_csv("./example_data/toy/beta_c1.csv",CRR.GetBeta());

//    m(0, 0) = 3;
//    m(1, 0) = 2.5;
//    m(0, 1) = -1;
//    m(1,1)  = m(1,0)+m(0,1);
//    std::cout<<y.rows()<<y.cols()<<std::endl;
    // Eigen::MatrixXd beta = (X.transpose()*X).ldlt().solve(X.transpose()*y);

    return 0;
//
//    m.transposeInPlace();
//    cout<<m<<endl;
//    m.resize(1,4);
//    cout<<n<<endl;
}


#include <string>
#include <algorithm>
#include <vector>
#include <array>
#include <memory>
#include <map>
#include <cassert>
#include <fstream>
#include "DatasetARX.h"
#include "GaussSeq.h"
#include "ZeroSeq.h"
#include "utils.h"
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/multiprecision/cpp_dec_float.hpp>
using utils::my_float;
using boost::random::uniform_real_distribution;

/*
 *  Member variables
 */
boost::random::mt19937 DatasetARX::gen {};      // static generator

/*
 *  Helper member functions
 */

// Seed previous values from uniform distribution
void DatasetARX::seed_features() {
    uniform_real_distribution<my_float> u_seed(-1, 1);

    feat_5[0] = u_seed(gen);
    for (auto i = 0; i < 3; i++) {
        feat_10[i] = u_seed(gen);
    }
    for (auto i = 0; i < 5; i++) {
        feat_20[i] = u_seed(gen);
    }
}
void DatasetARX::noise_one() {
    this->seed_features();
    for (auto i = 0; i < utils::FEAT_COUNT; i++) {
        feats.push_back(std::make_unique<GaussSeq>());
    }
}
void DatasetARX::noise_two() {
    this->seed_features();

    uniform_real_distribution<my_float> u_mean(-10, 10);
    uniform_real_distribution<my_float> u_std(0.1, 5);

    for (auto i = 0; i < utils::FEAT_COUNT; i++) {
        feats.push_back(std::make_unique<GaussSeq>(u_mean(gen), u_std(gen)));
    }
}
void DatasetARX::noise_three() {
    this->seed_features();

    uniform_real_distribution<my_float> u_const(0, 1);
    uniform_real_distribution<my_float> u_coeff(-0.9, 0.9);
    uniform_real_distribution<my_float> u_seed(0, 1);
    std::vector<my_float> seed_vect {u_seed(gen),};
    std::map<int, my_float> coeff_map {
        {1, u_coeff(gen)},
    };

    for (auto i = 0; i < utils::FEAT_COUNT; i++) {
        // prep seq backing
        coeff_map[1] = u_coeff(gen);
        seed_vect[0] = u_seed(gen);

        ARSeq in_seq(coeff_map, u_const(gen));
        in_seq.seed_prev_vals(seed_vect);

        feats.push_back(std::make_unique<ARSeq>(in_seq));
    }
}
void DatasetARX::noise_four() {
    this->seed_features();

    uniform_real_distribution<my_float> u_const(-1000000, -999999);
    uniform_real_distribution<my_float> u_coeff(1.1, 2);
    uniform_real_distribution<my_float> u_seed(0, 0.0000001);
    std::vector<my_float> seed_vect {u_seed(gen),};
    std::map<int, my_float> coeff_map {
        {1, u_coeff(gen)},
    };

    for (auto i = 0; i < utils::FEAT_COUNT; i++) {
        // prep seq backing
        coeff_map[1] = u_coeff(gen);
        seed_vect[0] = u_seed(gen);

        ARSeq in_seq(coeff_map, u_const(gen));
        in_seq.seed_prev_vals(seed_vect);

        feats.push_back(std::make_unique<ARSeq>(in_seq));
    }
}

/*
 *  Constructors and destructors
 */
DatasetARX::DatasetARX(std::string file_name):
    fname{file_name.append(".csv")}
    {};

DatasetARX::DatasetARX(std::string file_name,  unsigned char type):
    fname{file_name.append(".csv")},
    noise_type{type} {
    set_noise(type);
}

/*
 *  Member functions
 */
void DatasetARX::set_noise(unsigned char type) {
    this->noise_type = type;

    switch (type) {
        case 1: this->noise_one();
                break;
        case 2: this->noise_two();
                break;
        case 3: this->noise_three();
                break;
        case 4: this->noise_four();
                break;
    }
}
std::array<std::array<my_float, utils::FEAT_COUNT + 1>, utils::TIME_STEP>
    DatasetARX::generate_normalized_dat() {
    namespace mp = boost::multiprecision;
    constexpr int T = utils::TIME_STEP;
    constexpr int F = utils::FEAT_COUNT;
    // observe storage[feat][time] indexing
    std::array<std::array<my_float, T + 1>, F + 1> storage {};

    /*
     *  Prepare progress bar
     */
    // offset used to determine when to update progress bar
    int offset_mod = static_cast<int>(T / 10);
    int offset_counter {0};
    // print initial progress bar
    std::cout << "[";
    for (auto i = 0; i < 10; i++) {
        std::cout << " ";
    }
    std::cout << "]\r";

    /*
     *  Store non-normalized values
     */

    // Store time-delayed values
    // If of noise types 3 or 4, then there is one previous value as a ARSeq.
    // Otherwise, simply invoke next()
    if (this->noise_type == 3 || this->noise_type == 4) {
        for (auto f = 0; f < F + 1; f++) {
            if (f == F)
                storage[f][0] = 0.1*feat_5[0] + 0.25*feat_10[2] + 0.5*feat_20[4] +
                    error_term.next();
            else
                storage[f][0] = dynamic_cast<ARSeq*>(feats[f].get())->get_prev_val()[0];
        }
    } else {
        for (auto f = 0; f < F + 1; f++) {
            if (f == F)
                storage[f][0] = 0.1*feat_5[0] + 0.25*feat_10[2] + 0.5*feat_20[4] +
                    error_term.next();
            else
                storage[f][0] = feats[f]->next();
        }
    }

    // Store current values (those non-negative)
    my_float temp_feat_5 {0};
    my_float temp_feat_10 {0};
    my_float temp_feat_20 {0};

    for (auto t = 1; t < T + 1; t++) {
        // print progress bar
        if ((t - 1) % offset_mod == 0) {
            offset_counter++;

            std::cout << "[";
            for (auto i = 0; i < offset_counter; i++) {
                std::cout << "X";
            }
            for (auto i = 0; i < 10 - offset_counter; i++) {
                std::cout << " ";
            }
            std::cout << "]\r";
        }

        // write and record sequences
        for (auto f = 0; f < F + 1; f++) {
            // target column
            if (f == F) {
                storage[f][t] = 0.1*feat_5[0] + 0.25*feat_10[2] + 0.5*feat_20[4] +
                    error_term.next();

                // update past values
                utils::shift_vector(this->feat_5);
                utils::shift_vector(this->feat_10);
                utils::shift_vector(this->feat_20);

                feat_5[0] = temp_feat_5;
                feat_10[0] = temp_feat_10;
                feat_20[0] = temp_feat_20;
            } else if (f == 5) {
                temp_feat_5 = feats[f]->next();
                storage[f][t] = temp_feat_5;
            } else if (f == 10) {
                temp_feat_10 = feats[f]->next();
                storage[f][t] = temp_feat_10;
            } else if (f == 20) {
                temp_feat_20 = feats[f]->next();
                storage[f][t] = temp_feat_20;
            } else {
                storage[f][t] = feats[f]->next();
            }
        }
    }

    /*
     *  Normalize array values according to utils::NORM_VAL
     */
    my_float feat_min {0};
    my_float feat_max {0};
    // observe normalized_feats[time][feat] indexing
    std::array<std::array<my_float, F + 1>, T> normalized_feats {};

    // no need to check for zero values since we cannot use those with ARX
    for (auto f = 0; f < F + 1; f++) {
        feat_min = *std::min_element(storage[f].begin(), storage[f].end());
        feat_max = *std::max_element(storage[f].begin(), storage[f].end());

        for (auto t = 0; t < T; t++) {
            if (feat_max - feat_min == 0)
                normalized_feats[t][f] = (utils::NORM_VAL * 2 * (storage[f][t + 1] - feat_min)) -
                    utils::NORM_VAL;
            else
                normalized_feats[t][f] = (utils::NORM_VAL * 2 * ((storage[f][t + 1] - feat_min) /
                    (feat_max - feat_min))) - utils::NORM_VAL;
        }
    }

    return normalized_feats;
}

void DatasetARX::write_csv() {
    // was the noise value assigned?
    assert(this->noise_type != 10);

    // storage for normalize values
    constexpr int T = utils::TIME_STEP;
    constexpr int F = utils::FEAT_COUNT;
    std::array<std::array<my_float, F + 1>, T> storage = generate_normalized_dat();

    // initialize csv
    std::ofstream synth_data {this->fname};
    synth_data << std::setprecision(25);

    // prepare labels
    for (auto i = 0; i < utils::FEAT_COUNT + 2; i++) {
        if (i == 0)
            synth_data << "Time,";
        else if (i == utils::FEAT_COUNT + 1)
            synth_data << "Target\n";
        else
            synth_data << "Feature_" << i << ",";
    }

    // write values
    for (auto t = 0; t < T; t++) {
        for (auto f = 0; f < F + 2; f++) {
            // time column
            if (f == 0)
                synth_data << t << ",";
            // target column
            else if (f == utils::FEAT_COUNT + 1) {
                synth_data << storage[t][f - 1] << "\n";
            } else {
                synth_data << storage[t][f - 1] << ",";
            }
        }
    }
    std::cout << "\nFinished writing " << this->fname << "!\n";
    synth_data.close();
}


//
//  sall.cpp
//  Percy
//
//  Created by Linhai Zhao on 4/22/15.
//  Copyright (c) 2015 Linhai Zhao. All rights reserved.
//

#include <boost/python.hpp>
#include <math.h>
#include <map>
#include <string>
#include <vector>
#include <iostream>

using namespace std;
typedef std::vector<int> IntVector;
typedef std::vector<std::string> StringVector;
typedef std::vector<StringVector> VecStringVec;
typedef map<int, StringVector> MyMap;

/*int main(){
	double s_all;
    StringVector fallele;
    MyMap fam_allele;
    IntVector affnf;
    affnf={2,3};
    fallele={"gp0","gp1","gm0","gm1"};
    fam_allele[2]={"gp0","gm0"};
    fam_allele[3]={"gp1","gm0"};
    fam_allele[4]={"gp0","gm1"};
    s_all=sall(&affnf,&fam_allele, &fallele);
    return 0;
}*/

int factorial(int n){
    int fac=1;
    if (n==0) {
        return 1;
    }
    if (n<0) {
        return -99;
    }
    while (n>0) {
        fac = fac*n;
        n--;
    }
    return fac;
}

double sall(IntVector*aff, MyMap*fam_a,StringVector*fa, IntVector*fm,bool rv_flag){
    IntVector tmp_aff = *aff;
    int affnum = tmp_aff.size();
    double sall;
    MyMap fam_allele = *fam_a;
    StringVector founderallele=*fa;
    IntVector foundermarker=*fm;
    int total = pow(2.0,affnum);
    int total_count=0;
    for (int i=0; i<total; i++) {
        //for each unique selection of alleles
        //to binary;
        string bin;
        int number = i;
        char holder=' ';
        if (number==0) {
            bin='0';
        }
        while(number !=0){
            holder=number%2+'0';
            bin=holder+bin;
            number /=2;
        }
        //cout<<string((affnum-bin.size()),'0')<<endl;
        bin=string((affnum-bin.size()),'0')+bin;
        StringVector pick(affnum);
        //convert binary digits to founder alleles
        for (int j=0; j<bin.size(); j++) {
            int binj = bin[j]-'0'; //convert character to integer
            pick[j]=fam_allele[tmp_aff[j]][binj];
        }
        //h[i]=pick;
        int tmp_count = 1;
        //count the occurrences of founder alleles in picked alleles
        for (StringVector::iterator a=founderallele.begin(); a != founderallele.end(); ++a) {
            int occurrence=0;// = std::count(pick.begin(), pick.end(), *a);
	    if (rv_flag && foundermarker.at(a-founderallele.begin()) != 1 || !rv_flag){
			for (StringVector::iterator pit=pick.begin(); pit != pick.end();++pit){
				if (*pit == *a){occurrence += 1;}
				}
			}
            tmp_count *= factorial(occurrence);
        }
        total_count += tmp_count;
    }
    sall=double(total_count)/double(total);
	return sall;
}

IntVector toIntVec(boost::python::list aff){
	IntVector t_aff;
	for(int i =0;i<len(aff);++i){
		int tmp=boost::python::extract<int>(aff[i]);
		t_aff.push_back(tmp);
	}
	return t_aff;
}

StringVector toStrVec(boost::python::list fa){
	StringVector t_fa;
	for(int i = 0; i<len(fa);++i){
		string tmp=boost::python::extract<string>(fa[i]);
		t_fa.push_back(tmp);
	}
	return t_fa;
}

MyMap tomap(boost::python::dict nf){
	MyMap t_nf;
	boost::python::list keys = nf.keys();
	for(int i=0; i<len(keys);++i){
		int extracted_key=boost::python::extract<int>(keys[i]);
		StringVector extracted_val;
		extracted_val.push_back(boost::python::extract<string>(nf[extracted_key][0]));
		extracted_val.push_back(boost::python::extract<string>(nf[extracted_key][1]));
		t_nf[extracted_key]=extracted_val;
	}
	return t_nf;
}

double apply(boost::python::list aff, boost::python::dict fam_a, boost::python::list fa, boost::python::list fm, bool rv_flag){
	IntVector caff;
	StringVector fallele;
	IntVector fmarker;
	MyMap cfam_a;
	double s_all;
	caff = toIntVec(aff);
	fallele = toStrVec(fa);
	fmarker = toIntVec(fm);
	cfam_a = tomap(fam_a);
	s_all = sall(&caff,&cfam_a,&fallele,&fmarker,rv_flag);
	return s_all;
}

BOOST_PYTHON_MODULE(sall_cpp){
	using namespace boost::python;
	def("sall",sall);
	def("apply",apply);
}


// Copyright (c) 2018 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)

#include <boost/units2/unit.hpp>
#include <boost/units2/def.hpp>
#include <boost/type_index.hpp>

#define BOOST_TEST_MODULE test_unit
#include <boost/test/unit_test.hpp>

BOOST_UNITS2_DEF(length);
BOOST_UNITS2_DEF(meter, length);
BOOST_UNITS2_DEF(yard, meter * std::ratio<9144,10000>());
BOOST_UNITS2_DEF(foot, yard * std::ratio<1,3>());
BOOST_UNITS2_DEF(inch, foot * std::ratio<1,12>());
BOOST_UNITS2_DEF(xmeter, meter);
BOOST_UNITS2_DEF(sq_meter, meter * meter);

inline constexpr auto centimeter = std::centi() * meter;

struct degree_factor : boost::units2::scale_base {
    static constexpr double value() { return 180/3.14159265358979323846; }
};
BOOST_UNITS2_DEF(angle);
BOOST_UNITS2_DEF(radian, angle);
BOOST_UNITS2_DEF(degree, degree_factor()*radian);

#define TEST_SAME_TYPE(T, U) BOOST_TEST(::boost::typeindex::type_id<decltype(T)>() == ::boost::typeindex::type_id<decltype(U)>())
#define TEST_NOT_SAME_TYPE(T, U) BOOST_TEST(::boost::typeindex::type_id<decltype(T)>() != ::boost::typeindex::type_id<decltype(U)>())

BOOST_AUTO_TEST_CASE(test_multiply)
{
    // Multiplication should yield the same type regardless of argument order.
    TEST_SAME_TYPE(meter * yard, yard * meter);

    // sq_meter is a distinct type from meter*meter.
    TEST_NOT_SAME_TYPE(meter * meter * meter, sq_meter * meter);

    // Multiplication by a std::ratio is defined
    TEST_SAME_TYPE(meter * std::centi(), centimeter);
    TEST_SAME_TYPE(std::centi() * meter, centimeter);

    // Scale factors should collapse...
    TEST_SAME_TYPE(std::deci() * centimeter, std::milli() * meter);
    TEST_SAME_TYPE(centimeter * std::deci(), std::milli() * meter);
    // ...and should be removed entirely when they cancel
    TEST_SAME_TYPE(std::hecto() * centimeter, meter);
    TEST_SAME_TYPE(centimeter * std::hecto(), meter);

    // The scale factor should be reduced to its lowest terms
    TEST_SAME_TYPE((std::ratio<4,2>() * meter), (meter * std::ratio<6,3>()));

    // A scale factor of 1 is ignored
    TEST_SAME_TYPE((std::ratio<1,1>() * meter), meter);
    TEST_SAME_TYPE((meter * std::ratio<1,1>()), meter);
    // ...even if the ratio is only equivalent to 1.
    TEST_SAME_TYPE((std::ratio<3,3>() * meter), meter);
    TEST_SAME_TYPE((meter * std::ratio<3,3>()), meter);
    // ...and it is not ambiguous with folding scale factors.
    TEST_SAME_TYPE((std::ratio<3,3>() * centimeter), centimeter);
    TEST_SAME_TYPE((centimeter * std::ratio<3,3>()), centimeter);
}

// Everything should be calculated using exact arithmetic up to the
// final division.  Therfore, the maximum possible difference is 1 ulp.
BOOST_AUTO_TEST_CASE(test_basic_conversion, * boost::unit_test::tolerance(std::numeric_limits<double>::epsilon()))
{
    // converting a unit to itself always yields a factor of 1.
    BOOST_TEST(conversion_factor(meter, meter) == 1.0);
    BOOST_TEST(conversion_factor(centimeter, centimeter) == 1.0);
    BOOST_TEST(conversion_factor(inch, inch) == 1.0);

    // Normalizing the dimensions should work when a base unit
    // directly uses another base unit.
    BOOST_TEST(conversion_factor(xmeter, meter) == 1.0);

    // conversions should work in both directions
    BOOST_TEST(conversion_factor(inch, centimeter) == 2.54);
    BOOST_TEST(conversion_factor(centimeter, inch) == 100./254);

    // Composite conversions should work.
    BOOST_TEST(conversion_factor(meter * meter, inch * foot), 32.80839895013123358);

    // The result should be correct even when it is too large or too small to
    // be calculated using std::ratio.
    auto nm = std::nano() * meter;
    BOOST_TEST(conversion_factor(nm*nm*nm, meter*meter*meter) == 1e-27);
    BOOST_TEST(conversion_factor(meter*meter*meter, nm*nm*nm) == 1e+27);
}


//
// Copyright (c) 2017 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid 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 GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include <tsid/math/utils.hpp>
#include <tsid/robots/robot-wrapper.hpp>

#include <tsid/tasks/task-se3-equality.hpp>
#include <tsid/tasks/task-com-equality.hpp>
#include <tsid/tasks/task-joint-posture.hpp>
#include <tsid/tasks/task-joint-bounds.hpp>
#include <tsid/tasks/task-joint-posVelAcc-bounds.hpp>

#include <tsid/trajectories/trajectory-se3.hpp>
#include <tsid/trajectories/trajectory-euclidian.hpp>

#include <pinocchio/parsers/srdf.hpp>
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/algorithm/center-of-mass.hpp>
#include <Eigen/SVD>

using namespace tsid;
using namespace trajectories;
using namespace math;
using namespace tasks;
using namespace std;
using namespace Eigen;
using namespace tsid::robots;

#define REQUIRE_FINITE(A) BOOST_REQUIRE_MESSAGE(isFinite(A), #A<<": "<<A)

const string romeo_model_path = TSID_SOURCE_DIR"/models/romeo";

#ifndef NDEBUG
const int max_it = 100;
#else
const int max_it = 10000;
#endif

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_task_se3_equality )
{
  cout<<"\n\n*********** TEST TASK SE3 EQUALITY ***********\n";
  vector<string> package_dirs;
  package_dirs.push_back(romeo_model_path);
  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
  RobotWrapper robot(urdfFileName,
                     package_dirs,
                     pinocchio::JointModelFreeFlyer(),
                     false);

  TaskSE3Equality task("task-se3", robot, "RWristPitch");

  VectorXd Kp = VectorXd::Ones(6);
  VectorXd Kd = 2*VectorXd::Ones(6);
  task.Kp(Kp);
  task.Kd(Kd);
  BOOST_CHECK(task.Kp().isApprox(Kp));
  BOOST_CHECK(task.Kd().isApprox(Kd));

  pinocchio::SE3 M_ref = pinocchio::SE3::Random();
  TrajectoryBase *traj = new TrajectorySE3Constant("traj_SE3", M_ref);
  TrajectorySample sample;

  double t = 0.0;
  const double dt = 0.001;
  MatrixXd Jpinv(robot.nv(), 6);
  double error, error_past=1e100;
  VectorXd q = neutral(robot.model());
  VectorXd v = VectorXd::Zero(robot.nv());
  pinocchio::Data data(robot.model());
  for(int i=0; i<max_it; i++)
  {
    robot.computeAllTerms(data, q, v);
    sample = traj->computeNext();
    task.setReference(sample);
    const ConstraintBase & constraint = task.compute(t, q, v, data);
    BOOST_CHECK(constraint.rows()==6);
    BOOST_CHECK(static_cast<tsid::math::Index>(constraint.cols())==static_cast<tsid::math::Index>(robot.nv()));
    REQUIRE_FINITE(constraint.matrix());
    BOOST_REQUIRE(isFinite(constraint.vector()));

    pseudoInverse(constraint.matrix(), Jpinv, 1e-4);
    Vector dv = Jpinv * constraint.vector();
    BOOST_REQUIRE(isFinite(Jpinv));
    BOOST_CHECK(MatrixXd::Identity(6,6).isApprox(constraint.matrix()*Jpinv));
    if(!isFinite(dv))
    {
      cout<< "Jpinv" << Jpinv.transpose() <<endl;
      cout<< "b" << constraint.vector().transpose() <<endl;
    }
    REQUIRE_FINITE(dv.transpose());

    v += dt*dv;
    q = pinocchio::integrate(robot.model(), q, dt*v);
    BOOST_REQUIRE(isFinite(v));
    BOOST_REQUIRE(isFinite(q));
    t += dt;

    error = task.position_error().norm();
    BOOST_REQUIRE(isFinite(task.position_error()));
    BOOST_CHECK(error <= error_past);
    error_past = error;

    if(i%100==0)
      cout << "Time "<<t<<"\t Pos error "<<error<<
              "\t Vel error "<<task.velocity_error().norm()<<endl;
  }
}

BOOST_AUTO_TEST_CASE ( test_task_com_equality )
{
  cout<<"\n\n*********** TEST TASK COM EQUALITY ***********\n";
  vector<string> package_dirs;
  package_dirs.push_back(romeo_model_path);
  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
  RobotWrapper robot(urdfFileName,
                     package_dirs,
                     pinocchio::JointModelFreeFlyer(),
                     false);

  pinocchio::Data data(robot.model());
  const string srdfFileName = package_dirs[0] + "/srdf/romeo_collision.srdf";

  pinocchio::srdf::loadReferenceConfigurations(robot.model(),srdfFileName,false);

  //  const unsigned int nv = robot.nv();
  VectorXd q = neutral(robot.model());
  std::cout << "q: " << q.transpose() << std::endl;
  q(2) += 0.84;

  pinocchio::centerOfMass(robot.model(),data,q);

  TaskComEquality task("task-com", robot);

  VectorXd Kp = VectorXd::Ones(3);
  VectorXd Kd = 2.0*VectorXd::Ones(3);
  task.Kp(Kp);
  task.Kd(Kd);
  BOOST_CHECK(task.Kp().isApprox(Kp));
  BOOST_CHECK(task.Kd().isApprox(Kd));

  Vector3 com_ref = data.com[0] + pinocchio::SE3::Vector3(0.02,0.02,0.02);
  TrajectoryBase *traj = new TrajectoryEuclidianConstant("traj_com", com_ref);
  TrajectorySample sample;

  double t = 0.0;
  const double dt = 0.001;
  MatrixXd Jpinv(robot.nv(), 3);
  double error, error_past=1e100;
  VectorXd v = VectorXd::Zero(robot.nv());
  for(int i=0; i<max_it; i++)
  {
    robot.computeAllTerms(data, q, v);
    sample = traj->computeNext();
    task.setReference(sample);
    const ConstraintBase & constraint = task.compute(t, q, v, data);
    BOOST_CHECK(constraint.rows()==3);
    BOOST_CHECK(static_cast<tsid::math::Index>(constraint.cols())==static_cast<tsid::math::Index>(robot.nv()));
    BOOST_REQUIRE(isFinite(constraint.matrix()));
    BOOST_REQUIRE(isFinite(constraint.vector()));

    pseudoInverse(constraint.matrix(), Jpinv, 1e-5);
    Vector dv = Jpinv * constraint.vector();
    BOOST_REQUIRE(isFinite(Jpinv));
    BOOST_CHECK(MatrixXd::Identity(constraint.rows(),constraint.rows()).isApprox(constraint.matrix()*Jpinv));
    BOOST_REQUIRE(isFinite(dv));

    v += dt*dv;
    q = pinocchio::integrate(robot.model(), q, dt*v);
    BOOST_REQUIRE(isFinite(v));
    BOOST_REQUIRE(isFinite(q));
    t += dt;

    error = task.position_error().norm();
    BOOST_REQUIRE(isFinite(task.position_error()));
    BOOST_CHECK((error - error_past) <= 1e-4);
    error_past = error;

    if(error < 1e-8) break;

    if(i%100==0)
      cout << "Time "<<t<<"\t CoM pos error "<<error<<
              "\t CoM vel error "<<task.velocity_error().norm()<<endl;
  }
}

BOOST_AUTO_TEST_CASE ( test_task_joint_posture )
{
  cout<<"\n\n*********** TEST TASK JOINT POSTURE ***********\n";
  vector<string> package_dirs;
  package_dirs.push_back(romeo_model_path);
  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
  RobotWrapper robot(urdfFileName,
                     package_dirs,
                     pinocchio::JointModelFreeFlyer(),
                     false);
  const unsigned int na = robot.nv()-6;

  cout<<"Gonna create task\n";
  TaskJointPosture task("task-posture", robot);

  cout<<"Gonna set gains\n"<<na<<endl;
  VectorXd Kp = VectorXd::Ones(na);
  VectorXd Kd = 2.0*Kp;
  task.Kp(Kp);
  task.Kd(Kd);
  BOOST_CHECK(task.Kp().isApprox(Kp));
  BOOST_CHECK(task.Kd().isApprox(Kd));

  cout<<"Gonna create reference trajectory\n";
  Vector q_ref = Vector::Random(na);
  TrajectoryBase *traj = new TrajectoryEuclidianConstant("traj_joint", q_ref);
  TrajectorySample sample;

  cout<<"Gonna set up for simulation\n";
  double t = 0.0;
  const double dt = 0.001;
  MatrixXd Jpinv(robot.nv(), na);
  double error, error_past=1e100;
  VectorXd q = neutral(robot.model());
  VectorXd v = VectorXd::Zero(robot.nv());
  pinocchio::Data data(robot.model());
  for(int i=0; i<max_it; i++)
  {
    robot.computeAllTerms(data, q, v);
    sample = traj->computeNext();
    task.setReference(sample);
    const ConstraintBase & constraint = task.compute(t, q, v, data);
    BOOST_CHECK(constraint.rows()==na);
    BOOST_CHECK(static_cast<tsid::math::Index>(constraint.cols())==static_cast<tsid::math::Index>(robot.nv()));
    BOOST_REQUIRE(isFinite(constraint.matrix()));
    BOOST_REQUIRE(isFinite(constraint.vector()));

    pseudoInverse(constraint.matrix(), Jpinv, 1e-5);
    Vector dv = Jpinv * constraint.vector();
    BOOST_REQUIRE(isFinite(Jpinv));
    BOOST_CHECK(MatrixXd::Identity(na,na).isApprox(constraint.matrix()*Jpinv));
    BOOST_REQUIRE(isFinite(dv));

    v += dt*dv;
    q = pinocchio::integrate(robot.model(), q, dt*v);
    BOOST_REQUIRE(isFinite(v));
    BOOST_REQUIRE(isFinite(q));
    t += dt;

    error = task.position_error().norm();
    BOOST_REQUIRE(isFinite(task.position_error()));
    BOOST_CHECK(error <= error_past);
    error_past = error;

    if(i%100==0)
      cout << "Time "<<t<<"\t pos error "<<error<<
              "\t vel error "<<task.velocity_error().norm()<<endl;
  }
}

BOOST_AUTO_TEST_CASE ( test_task_joint_bounds )
{
  cout<<"\n\n*********** TEST TASK JOINT BOUNDS ***********\n";
  vector<string> package_dirs;
  package_dirs.push_back(romeo_model_path);
  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
  RobotWrapper robot(urdfFileName,
                     package_dirs,
                     pinocchio::JointModelFreeFlyer(),
                     false);
  const unsigned int na = robot.nv()-6;
  const double dt = 0.001;

  cout<<"Gonna create task\n";
  TaskJointBounds task("task-joint-bounds", robot, dt);

  cout<<"Gonna set limits\n"<<na<<endl;
  VectorXd dq_max = VectorXd::Ones(na);
  VectorXd dq_min = -dq_max;
  task.setVelocityBounds(dq_min, dq_max);

  BOOST_CHECK(task.getVelocityLowerBounds().isApprox(dq_min));
  BOOST_CHECK(task.getVelocityUpperBounds().isApprox(dq_max));

  cout<<"Gonna set up for simulation\n";
  double t = 0.0;

  VectorXd q = neutral(robot.model());
  VectorXd v = VectorXd::Zero(robot.nv());
  pinocchio::Data data(robot.model());
  for(int i=0; i<max_it; i++)
  {
    robot.computeAllTerms(data, q, v);
    const ConstraintBase & constraint = task.compute(t, q, v, data);
    BOOST_CHECK(constraint.rows()==(Eigen::Index)robot.nv());
    BOOST_CHECK(static_cast<tsid::math::Index>(constraint.cols())==static_cast<tsid::math::Index>(robot.nv()));
    BOOST_REQUIRE(isFinite(constraint.lowerBound()));
    BOOST_REQUIRE(isFinite(constraint.upperBound()));

    BOOST_REQUIRE(isFinite(v));
    BOOST_REQUIRE(isFinite(q));
    t += dt;
  }
}

BOOST_AUTO_TEST_CASE ( test_task_joint_posVelAcc_bounds )
{
  cout<<"\n\n*********** TEST TASK JOINT POS VEL ACC BOUNDS ***********\n";
  vector<string> package_dirs;
  package_dirs.push_back(romeo_model_path);
  string urdfFileName = package_dirs[0] + "/urdf/romeo.urdf";
  RobotWrapper robot(urdfFileName,
                     package_dirs,
                     pinocchio::JointModelFreeFlyer(),
                     false);
  const unsigned int na = robot.nv()-6;
  const double dt = 0.001;

  cout<<"Gonna create task\n";
  TaskJointPosVelAccBounds task("task-joint-posVelAcc-bounds", robot, dt);

  cout<<"Gonna set limits\n"<<na<<endl;
  VectorXd dq_max = VectorXd::Ones(na);
  VectorXd dq_min = -dq_max;

  task.setPositionBounds(dq_min,dq_max);
  task.setVelocityBounds(dq_max);
  task.setAccelerationBounds(dq_max);

  BOOST_CHECK(task.getPositionLowerBounds().isApprox(dq_min));
  BOOST_CHECK(task.getPositionUpperBounds().isApprox(dq_max));
  BOOST_CHECK(task.getVelocityBounds().isApprox(dq_max));
  BOOST_CHECK(task.getAccelerationBounds().isApprox(dq_max));

  cout<<"Gonna set up for simulation\n";
  double t = 0.0;

  VectorXd q = neutral(robot.model());
  VectorXd v = VectorXd::Zero(robot.nv());
  pinocchio::Data data(robot.model());
  for(int i=0; i<max_it; i++)
  {
    robot.computeAllTerms(data, q, v);
    const ConstraintBase & constraint = task.compute(t, q, v, data);
    BOOST_CHECK(constraint.rows()==(Eigen::Index)robot.na());
    BOOST_CHECK(static_cast<tsid::math::Index>(constraint.cols())==static_cast<tsid::math::Index>(robot.nv()));
    BOOST_REQUIRE(isFinite(constraint.lowerBound()));
    BOOST_REQUIRE(isFinite(constraint.upperBound()));

    BOOST_REQUIRE(isFinite(v));
    BOOST_REQUIRE(isFinite(q));
    t += dt;
  }
}

BOOST_AUTO_TEST_SUITE_END ()


/*-----------------------------------------------------------------------------+
Copyright (c) 2007-2009: Joachim Faulhaber
+------------------------------------------------------------------------------+
Copyright (c) 1999-2006: Cortex Software GmbH, Kantstrasse 57, Berlin
+------------------------------------------------------------------------------+
   Distributed under the Boost Software License, Version 1.0.
      (See accompanying file LICENCE.txt or copy at
           http://www.boost.org/LICENSE_1_0.txt)
+-----------------------------------------------------------------------------*/

/*-----------------------------------------------------------------------------+
splititvmap_shell.cpp provides  a simple test shell for splitting interval maps.
The shell also gives you a good idea how interval container are working.
+-----------------------------------------------------------------------------*/
#include <iostream>

#include <boost/icl/split_interval_set.hpp>
#include <boost/icl/split_interval_map.hpp>
#include <boost/icl/interval_map.hpp>

using namespace std;
using namespace boost;
using namespace boost::icl;

void instructions()
{
    cout << "+++++ Test shell for split interval map +++++\n";
    cout << "Type: q e or 0  to quit\n";
    cout << "Type: +         for insertions\n";
    cout << "Type: -         for subtraction of ([a,b],value)\n";
    cout << "Type: _         for subtraction of [a,b]\n";
    cout << "Type: j         to join contiguous intervals\n";
    cout << "Type: s         to compute total size\n";
}

void wrongInput()
{
    cout << "Wrong Input ------------------\n";
    instructions();
}

template <class MapTV>
void mapTestShell()
{
    MapTV m1;

    try {
        char cmd = 'b';
        typename MapTV::domain_type
            lwb = typename MapTV::domain_type(),
            upb = typename MapTV::domain_type();

        typename MapTV::codomain_type
            val = typename MapTV::codomain_type();

        instructions();

        for(;;)
        {
            cout << "> ";
            cin >> cmd ;

            switch(cmd)
            {
            case 'q':
            case 'e':
            case '0': cout << "good bye\n"; return;
            case '+':
                {
                    cout << "input: lwb upb val >> ";
                    cin >> lwb >> upb >> val;
                    typename MapTV::interval_type
                        itv = typename MapTV::interval_type(lwb,upb);
                    m1 += make_pair(itv,val);

                    cout << "+" << itv << " " << val << " =" << endl;
                    cout << "{" << m1 << "}" << endl;

                }
                break;
            case '-':
                {
                    cout << "input: lwb upb val >> ";
                    cin >> lwb >> upb >> val;
                    typename MapTV::interval_type
                        itv = typename MapTV::interval_type(lwb,upb);
                    m1 -= make_pair(itv,val);

                    cout << "-" << itv << " " << val << " =" << endl;
                    cout << "{" << m1 << "}" << endl;

                }
                break;
            case 'j':
                {
                    icl::join(m1);
                    cout << "{" << m1 << "}" << endl;
                }
                break;
            case 's':
                {
                    cout << "size = " << m1.size() << endl;
                }
                break;

            default: wrongInput();
            }
        } // end while
    }
    catch (exception& e)
    {
        cout << "splititvmap_shell: exception caught: " << endl
             << e.what() << endl;
    }
    catch (...)
    {
        cout << "splititvmap_shell: unknown exception caught" << endl;
    }
}

int main()
{
    cout << ">>Interval Container Library: Test splititvmap_shell.cpp <<\n";
    cout << "-----------------------------------------------------------\n";
    mapTestShell< interval_map<int, int> >();

    return 0;
}


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

#include <iostream>
#include <boost/numeric/mtl/mtl.hpp>
#include <boost/timer.hpp>

using namespace std;
using namespace mtl;

typedef compressed2D<double> sp_mat;

void assemble(sp_mat& A, double val)
{
  mat::inserter<sp_mat, update_plus<double> > ins(A, 3);

  double array[][3]= {{-2*val, val, val},
		      {val, -2*val, val},
		      {val, val, -2*val}};

  dense2D<double> block(array);
  dense_vector<int> cols(3);
  dense_vector<int> rows(3);

  int N= num_rows(A); // A is N-by-N
  for(int k=0; k<N-2; k+=3)    // nice case
  //for(int k=N-3; k>=0; k-=3) // out-of-order case
    {
      rows[0] = k; rows[1] = k+1; rows[2] = k+2;
      cols[0] = 0; cols[1] = k;   cols[2] = N-1;

      ins << element_matrix(block, rows, cols);
    }
}

int main(int argc, char* argv[])
{
    sp_mat B(9, 9);
    assemble(B, 1.0);
    cout << "Small assembled matrix\n" << B << "\n";

    const int size= 900000;
    sp_mat A(size, size);

    boost::timer atime;
    assemble(A, 1.0);
    cout << "Assemble time = " << atime.elapsed() << ", "
	 << 3*size / atime.elapsed() << " elements/s\n";

    A*= 0.0;
    boost::timer rtime;
    assemble(A, 1.0);
    cout << "Reassemble time = " << rtime.elapsed() << ", "
	 << 3*size / rtime.elapsed() << " elements/s\n";

    return 0;
}