//#ifndef DO_DEBUG_PROC
//#define DO_DEBUG_PROC
//#endif

#include <ndt_matcher_p2d.h>
#include <ndt_matcher_d2d.h>
//#include <ndt_matcher_d2d.hh>
#include <ndt_map.h>
#include <oc_tree.h>
#include <pointcloud_utils.h>

#include "ros/ros.h"
#include "pcl/point_cloud.h"
#include "sensor_msgs/PointCloud2.h"
#include "pcl/io/pcd_io.h"
#include "pcl/features/feature.h"
#include "pcl/registration/icp.h"
#include "pcl/filters/voxel_grid.h"
#include <pcl/filters/passthrough.h>

#include <cstdio>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <fstream>

using namespace std;

/*
static int ctr = 0;
bool matchICP(pcl::PointCloud<pcl::PointXYZ> &fixed,  pcl::PointCloud<pcl::PointXYZ> &moving,
	      Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tout) {

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::ConstPtr f (new pcl::PointCloud<pcl::PointXYZ>(fixed) );
    pcl::PointCloud<pcl::PointXYZ>::ConstPtr m (new pcl::PointCloud<pcl::PointXYZ>(moving) );

    pcl::VoxelGrid<pcl::PointXYZ> gr1,gr2;
    gr1.setLeafSize(0.1,0.1,0.1);
    gr2.setLeafSize(0.1,0.1,0.1);

    gr1.setInputCloud(m);
    gr2.setInputCloud(f);

    cloud_in->height = 1;
    cloud_in->width = cloud_in->points.size();
    cloud_out->height = 1;
    cloud_out->width = cloud_out->points.size();
    cloud_in->is_dense = false;
    cloud_out->is_dense = false;

    gr1.filter(*cloud_in);
    gr2.filter(*cloud_out);

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;

    icp.setMaximumIterations(10000);
    cout<<"max itr are "<<icp.getMaximumIterations()<<endl;
    icp.setInputCloud(cloud_in);
    icp.setInputTarget(cloud_out);

    icp.setRANSACOutlierRejectionThreshold (2);
    icp.setMaxCorrespondenceDistance(10);
    icp.setTransformationEpsilon(0.00001);
//    cout<<"ransac outlier thersh   : "<<icp.getRANSACOutlierRejectionThreshold ()<<endl;
//    cout<<"correspondance max dist : "<<icp.getMaxCorrespondenceDistance() << endl;
//    cout<<"epsilon : "<<icp.getTransformationEpsilon() << endl;
    pcl::PointCloud<pcl::PointXYZ> Final;
    icp.align(Final);

//    std::cout << "has converged:" << icp.hasConverged() << " score: " <<
//	icp.getFitnessScore() << std::endl;
//    std::cout << icp.getFinalTransformation() << std::endl;

    //Eigen::Transform<float,3,Eigen::Affine,Eigen::ColMajor> tTemp;
    Tout = (icp.getFinalTransformation()).cast<double>();

    char fname[50];
    snprintf(fname,49,"/home/tsv/ndt_tmp/c2_offset.wrl");
    FILE *fout = fopen(fname,"w");
    fprintf(fout,"#VRML V2.0 utf8\n");
    lslgeneric::writeToVRML(fout,*cloud_out,Eigen::Vector3d(0,1,0));
    lslgeneric::writeToVRML(fout,Final,Eigen::Vector3d(1,0,0));
    lslgeneric::writeToVRML(fout,*cloud_in,Eigen::Vector3d(1,1,1));
    fclose(fout);

    return icp.hasConverged();

}
*/

int
main (int argc, char** argv)
{

    double roll=0,pitch=0,yaw=0,zoffset=0;

    pcl::PointCloud<pcl::PointXYZ> cloud, cloud_offset, cloud_OFF;
    pcl::PCDReader reader;
    pcl::PCDWriter writer;
    char fname[50];
    FILE *fout;
    //double __res[] = {0.5};
    double __res[] = {0.5, 1, 2, 4};
    std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
    lslgeneric::NDTMatcherP2D<pcl::PointXYZ,pcl::PointXYZ> matcherP2D(resolutions);

    struct timeval tv_start,tv_end,tv_reg_start,tv_reg_end;

    Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tin, Tout, Tout2;
    Tout.setIdentity();
    if(argc == 3)
    {

        gettimeofday(&tv_start,NULL);
        //we do a single scan to scan registration
        //reader.read(argv[1],cloud);
        //reader.read(argv[2],cloud_offset);
        cloud = lslgeneric::readVRML<pcl::PointXYZ>(argv[1]);
        cloud_offset = lslgeneric::readVRML<pcl::PointXYZ>(argv[2]);
        /*
        Tin =  Eigen::Translation<double,3>(0.5,0.2,0)*
            Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitX()) *
            Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitY()) *
            Eigen::AngleAxis<double>(0.3,Eigen::Vector3d::UnitZ()) ;
            */
        //cloud_offset = lslgeneric::transformPointCloud(Tin,cloud);
        //cloud_offset = cloud;
        /*
        	snprintf(fname,49,"c_offset.wrl");
        	FILE *fout = fopen(fname,"w");
        	fprintf(fout,"#VRML V2.0 utf8\n");
        	lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud,Eigen::Vector3d(0,1,0));
        	lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(1,0,0));
        	fclose(fout);
        */
        lslgeneric::NDTMatcherD2D<pcl::PointXYZ,pcl::PointXYZ> matcherD2D(false, false, resolutions);
        bool ret = matcherD2D.match2D(cloud,cloud_offset,Tout);

        snprintf(fname,49,"c_offset.wrl");
        fout = fopen(fname,"w");
        fprintf(fout,"#VRML V2.0 utf8\n");
        lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud,Eigen::Vector3d(1,0,0));
        lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(1,1,1));

        lslgeneric::transformPointCloudInPlace<pcl::PointXYZ>(Tout,cloud_offset);
        std::cout<<Tout.matrix()<<std::endl;
        lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(0,1,0));
        fclose(fout);

        return 0;
        /**/
        Eigen::Vector3d sensor_origin;
        sensor_origin<<0,0,0;
        lslgeneric::NDTMap<pcl::PointXYZ> sourceNDT(new lslgeneric::LazyGrid<pcl::PointXYZ>(0.5));
        //sourceNDT.loadPointCloud(cloud);
        sourceNDT.addPointCloud(sensor_origin,cloud);
        sourceNDT.computeNDTCells();
        Tin.setIdentity();

        lslgeneric::NDTMap<pcl::PointXYZ> targetNDT(new lslgeneric::LazyGrid<pcl::PointXYZ>(0.5));
        targetNDT.loadPointCloud(cloud_offset);
        targetNDT.computeNDTCells();

        matcherD2D.match2D(sourceNDT,targetNDT,Tout);
        snprintf(fname,49,"c_offset.wrl");
        fout = fopen(fname,"w");
        fprintf(fout,"#VRML V2.0 utf8\n");
        lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud,Eigen::Vector3d(1,0,0));
        lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(1,1,1));
        lslgeneric::transformPointCloudInPlace<pcl::PointXYZ>(Tout,cloud_offset);
        std::cout<<Tout.matrix()<<std::endl;
        lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(0,1,0));
        fclose(fout);
    }
}
#if 0
std::vector<lslgeneric::NDTCell<pcl::PointXYZ>*> source = sourceNDT.pseudoTransformNDT(Tin);
int N_X=40;
int N_Y=40;
double xmin = -1, ymin = -1, yawmin = -0.5;
double dx = 0.05, dy = 0.05, dyaw = 0.01;
//Eigen::MatrixXd H1(N_X,N_Y), H2(N_X,N_Y);
Eigen::MatrixXd scores(N_X,N_Y);
Eigen::MatrixXd scores2(N_X,N_Y);
Eigen::MatrixXd gX(N_X,N_Y), gY(N_X,N_Y);
scores.setZero();
gX.setZero();
gY.setZero();
int N_SUCC = 0;
std::vector<double> et;
std::vector<double> er;
#pragma omp parallel
{
    lslgeneric::NDTMatcherD2D<pcl::PointXYZ,pcl::PointXYZ> matcherD2DLocal(false, false, resolutions);
    #pragma omp for
    for(int i=0; i<N_X; i++)   //xoffset=xmin;xoffset<xmin+N_X*dx;xoffset+=dx) {
    {
        double xoffset = xmin+i*dx;
        for(int q=0; q<N_Y; q++)   //yoffset=ymin;yoffset<ymin+N_Y*dy;yoffset+=dy) {
        {
            double yoffset = ymin +q*dy;
//		for(int j=0; j<N_Y; j++) { //yoffset=ymin;yoffset<ymin+N_Y*dy;yoffset+=dy) {
//		    double yawoffset = yawmin +j*dyaw;
//		    double xoffset = -0.5, yoffset=0.3, yawoffset=-0.2;
            double yawoffset=0;
            Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Tloc =
                Eigen::Translation<double,3>(xoffset,yoffset,0)*
                Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitX()) *
                Eigen::AngleAxis<double>(0,Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxis<double>(yawoffset,Eigen::Vector3d::UnitZ()) ;

            Eigen::Vector3d so = Tloc*sensor_origin;
            pcl::PointCloud<pcl::PointXYZ> cloudL = lslgeneric::transformPointCloud<pcl::PointXYZ>(Tloc,cloud_offset);
            lslgeneric::NDTMap<pcl::PointXYZ> targetNDT(new lslgeneric::LazyGrid<pcl::PointXYZ>(0.5));
            //targetNDT.loadPointCloud(cloudL);
            targetNDT.addPointCloud(so,cloudL);
            targetNDT.computeNDTCells();
            //matcherD2DLocal.match(sourceNDT,targetNDT,Tout);
            //Tout = Tloc*Tout;
            scores2(i,q) = matcherD2DLocal.scoreNDT_OM(sourceNDT,targetNDT);
            scores(i,q) = matcherD2DLocal.scoreNDT( source, targetNDT);
            //std::cout<<"score: "<<d<<std::endl;
            /*
            		    lslgeneric::transformPointCloudInPlace<pcl::PointXYZ>(Tout,cloudL);
            		    snprintf(fname,49,"c_offset_l.wrl");
            		    fout = fopen(fname,"w");
            		    fprintf(fout,"#VRML V2.0 utf8\n");
            		    lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud,Eigen::Vector3d(0,1,0));
            		    lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloudL,Eigen::Vector3d(1,0,0));
            		    fclose(fout);
            		    cout<<xoffset<<" "<<yoffset<<" "<<yawoffset<<std::endl;
            		    cout<<"E translation "<<Tout.translation().transpose()
            			<<" (norm) "<<Tout.translation().norm()<<endl;
            		    cout<<"E rotation "<<Tout.rotation().eulerAngles(0,1,2).transpose()
            			<<" (norm) "<<Tout.rotation().eulerAngles(0,1,2).norm()<<endl;

            		    et.push_back(Tout.translation().norm());
            		    er.push_back(Tout.rotation().eulerAngles(0,1,2).norm());
            		    if(Tout.translation().norm() < 0.2 && Tout.rotation().eulerAngles(0,1,2).norm() < 0.087) {
            			cout<<"OK\n";
            			N_SUCC++;
            		    }
            */

            /*
            Eigen::MatrixXd score_gradient(6,1);
            Eigen::MatrixXd Hessian(6,6);

            std::vector<lslgeneric::NDTCell<pcl::PointXYZ>*> source = sourceNDT.pseudoTransformNDT(Tlocal);

            scores(i,j) = matcherD2D.derivativesNDT( source, targetNDT, score_gradient, Hessian, false);
            gX(i,j) = score_gradient(0,0);
            gY(i,j) = score_gradient(1,0);
            //		Eigen::Matrix<double,6,1>  pose_increment_v;
            //		pose_increment_v = -Hessian.ldlt().solve(score_gradient);
            //		H1(i,j) = pose_increment_v(0);
            //		H2(i,j) = pose_increment_v(1);

            for(int q =0; q<source.size(); ++q) {
            delete source[q];
            }
             */
//		}
        }
    }
}

//	    cout<<"N_SUCC = "<<N_SUCC<<std::endl;
std::cout<< " S = ["<<scores<<"];\n";
std::cout<< " S2 = ["<<scores2<<"];\n";
//	std::cout<< " gX = ["<<gX<<"];\n";
//	std::cout<< " gY = ["<<gY<<"];\n";
//	std::cout<< " H1 = ["<<H1<<"];\n";
//	std::cout<< " H2 = ["<<H2<<"];\n";
return 0;
/*	pcl::PointCloud<pcl::PointXYZ> cloud1, cloud_offset1;
	for(int i=0; i<cloud.points.size(); i++) {
	    double d = sqrt(pow(cloud.points[i].x,2) + pow(cloud.points[i].y,2) +pow(cloud.points[i].z,2));
	    if(d<3 && d>1) {
		cloud1.points.push_back(cloud.points[i]);
	    }
	}
	for(int i=0; i<cloud_offset.points.size(); i++) {
	    double d = sqrt(pow(cloud_offset.points[i].x,2) + pow(cloud_offset.points[i].y,2) +pow(cloud_offset.points[i].z,2));
	    if(d<3 && d>1) {
		cloud_offset1.points.push_back(cloud_offset.points[i]);
	    }
	}
*/
//	bool ret = matcherP2D.match(cloud,cloud_offset,Tout2);

//	lslgeneric::NDTMatcherD2D<pcl::PointXYZ,pcl::PointXYZ> matcherD2D(false, false, resolutions);
gettimeofday(&tv_reg_start,NULL);
ret = matcherD2D.match(cloud,cloud_offset,Tout);
//bool ret = matcherD2D.match(targetNDT,sourceNDT,Tout);
//bool ret = matcherD2D.match(sourceNDT,targetNDT,Tout);
gettimeofday(&tv_reg_end,NULL);

lslgeneric::transformPointCloudInPlace<pcl::PointXYZ>(Tout,cloud_offset);
//	cout<<"translation "<<Tout.translation().transpose()<<endl;
//	cout<<"euler: "<<Tout.rotation().eulerAngles(0,1,2).transpose()<<endl;

// cloud_offset += cloud;
snprintf(fname,49,"c_offset.pcd");
// writer.write(fname, cloud_offset);

snprintf(fname,49,"c_offset.wrl");
fout = fopen(fname,"w");
fprintf(fout,"#VRML V2.0 utf8\n");
lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud,Eigen::Vector3d(0,1,0));
lslgeneric::writeToVRML<pcl::PointXYZ>(fout,cloud_offset,Eigen::Vector3d(1,0,0));
fclose(fout);

//	cout<<"initial registration done, saved into file c_offset.pcd\n";
//	cout<<"make sure to check that initial registration was successful!\n";

gettimeofday(&tv_end,NULL);
double tim = (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
double tim2 = (tv_reg_end.tv_sec-tv_reg_start.tv_sec)*1000.+(tv_reg_end.tv_usec-tv_reg_start.tv_usec)/1000.;

//	cout<<"OVERALL TIME: "<<tim<<" REG TIME "<<tim2<<endl;
return 0;
}
#if 0
if(!(argc == 2 || succ))
{
    cout<<"usage: ./test_ndt point_cloud1 [point_cloud2]\n";
    return -1;
}

cloud = lslgeneric::readVRML(argv[1]);

        char fname[50];
        FILE *fout;
        if(!succ)
{
    cloud_offset = cloud;
}
else
{
    cloud_offset = cloud_OFF;
}

//matcherF2F.generateScoreDebug("/home/tsv/ndt_tmp/scoresF2F.m",cloud,cloud_offset);
//matcherP2F.generateScoreDebug("/home/tsv/ndt_tmp/scoresP2F.m",cloud,cloud_offset);
//cout<<"Generated scores\n";
//return 0;

std::vector<double> times;
std::vector<int> success;
std::vector<double> et;
std::vector<double> er;

roll = 0;
       pitch =0;
              yaw = 40*M_PI/180;
                    xoffset = -2;
                              yoffset = -1.;
                                        zoffset =0;
                                                //for(roll = -2*M_PI/5;roll<M_PI/2;roll+=M_PI/5) {
                                                //for(pitch = -2*M_PI/5;pitch<M_PI/2;pitch+=M_PI/5) {
                                                for(yaw = -30*M_PI/180; yaw<=35*M_PI/180; yaw+=10*M_PI/180)
{
    logger2<<"%";
    for(xoffset=-1.5; xoffset<=1.5; xoffset+=0.5)
    {
        for(yoffset=-1.5; yoffset<=1.5; yoffset+=0.5)
        {
            /*
            for(yaw = -5*M_PI/180;yaw<=15*M_PI/180;yaw+=10*M_PI/180) {
            for(xoffset=-0.1;xoffset<=0.5;xoffset+=0.5) {
            for(yoffset=0;yoffset<=0.5;yoffset+=0.5) {
            */
            //for(zoffset = -1;zoffset<1;zoffset+=0.5) {

            Tin =  Eigen::Translation<double,3>(xoffset,yoffset,zoffset)*
                   Eigen::AngleAxis<double>(roll,Eigen::Vector3d::UnitX()) *
                   Eigen::AngleAxis<double>(pitch,Eigen::Vector3d::UnitY()) *
                   Eigen::AngleAxis<double>(yaw,Eigen::Vector3d::UnitZ()) ;

            //cout<<"translation \n"<<Tin.translation().transpose()<<endl;
            //cout<<"rotation \n"<<Tin.rotation()<<endl;

            cout<<roll<<" "<<pitch<<" "<<yaw<<" "<<xoffset<<" "<<yoffset<<" "<<zoffset<<endl;
            logger<<roll<<" "<<pitch<<" "<<yaw<<" "<<xoffset<<" "<<yoffset<<" "<<zoffset<<endl;

            Eigen::Vector3d out = Tin.rotation().eulerAngles(0,1,2);
            //cout<<"euler: "<<out<<endl;

            if(!succ)
            {
                cloud_offset = lslgeneric::transformPointCloud(Tin,cloud);
            }
            else
            {
                cloud_offset = lslgeneric::transformPointCloud(Tin,cloud_OFF);
            }

            /*snprintf(fname,49,"/home/tsv/ndt_tmp/cloud_init%05d.wrl",ctr);
            fout = fopen(fname,"w");
            fprintf(fout,"#VRML V2.0 utf8\n");
            lslgeneric::writeToVRML(fout,cloud_offset,Eigen::Vector3d(1,0,0));
            lslgeneric::writeToVRML(fout,cloud,Eigen::Vector3d(0,1,0));
            fclose(fout);
            */
            gettimeofday(&tv_start,NULL);
            //bool ret = matcherP2F.match(cloud,cloud_offset,Tout);
            bool ret = matcherF2F.match(cloud,cloud_offset,Tout);
            //bool ret = matchICP(cloud,cloud_offset,Tout);
            gettimeofday(&tv_end,NULL);

            double tim = (tv_end.tv_sec-tv_start.tv_sec)*1000.+(tv_end.tv_usec-tv_start.tv_usec)/1000.;
            logger<<"TIME: "<<tim<<endl;
            C_TIME+=tim;

            if(!ret)
            {
                logger<<">>>>>>> NO SOLUTION <<<<<<<<<\n";
                cout<<"NO SOLUTION\n";
                logger2<<"N ";
                N_FAIL++;
                times.push_back(tim);
                et.push_back(Tout.translation().norm());
                er.push_back(Tout.rotation().eulerAngles(0,1,2).norm());
            }
            else
            {
                /*logger<<"Input Transform: "<<endl;
                logger<<"translation "<<Tin.translation()<<endl;
                logger<<"rotation "<<Tin.rotation()<<endl;
                logger<<"translation "<<Tout.translation()<<endl;
                logger<<"rotation "<<Tout.rotation()<<endl;
                */
                //lslgeneric::transformPointCloudInPlace(Tout,cloud_offset);
                Eigen::Vector3d out = Tout.rotation().eulerAngles(0,1,2);
                logger<<"OUT: "<<out.transpose()<<endl;
                //cout<<"OUT: "<<out<<endl;
                logger<<"translation "<<Tout.translation().transpose()<<endl;
                //cout<<"translation "<<Tout.translation()<<endl;
                snprintf(fname,49,"/home/tsv/ndt_tmp/cloud_offset%05d.wrl",ctr);
                lslgeneric::transformPointCloudInPlace(Tout,cloud_offset);

                /*fout = fopen(fname,"w");
                fprintf(fout,"#VRML V2.0 utf8\n");
                lslgeneric::writeToVRML(fout,cloud_offset,Eigen::Vector3d(1,0,0));
                lslgeneric::writeToVRML(fout,cloud,Eigen::Vector3d(0,1,0));
                fclose(fout);
                */
                ctr++;

                Tout = Tin*Tout;
                logger<<"E translation "<<Tout.translation().transpose()
                <<" (norm) "<<Tout.translation().norm()<<endl;
                logger<<"E rotation "<<Tout.rotation().eulerAngles(0,1,2).transpose()
                <<" (norm) "<<Tout.rotation().eulerAngles(0,1,2).norm()<<endl;

                times.push_back(tim);
                et.push_back(Tout.translation().norm());
                er.push_back(Tout.rotation().eulerAngles(0,1,2).norm());

                if(Tout.translation().norm() < 0.2 && Tout.rotation().eulerAngles(0,1,2).norm() < 0.087)
                {
                    logger<<"OK\n";
                    logger2<<"O ";
                    cout<<"OK\n";
                    N_SUCC++;
                    success.push_back(0);
                }
                else if(Tout.translation().norm() < 1 && Tout.rotation().eulerAngles(0,1,2).norm() < 0.15)
                {
                    logger<<"ACK\n";
                    logger2<<"A ";
                    cout<<"ACK\n";
                    success.push_back(1);
                }
                else
                {
                    logger<<"FAIL\n";
                    logger2<<"F ";
                    cout<<"FAIL\n";
                    N_FAIL++;
                    success.push_back(2);
                }

                return 0;
                //cout<<"E translation "<<Tout.translation()<<endl;
                //cout<<"E rotation "<<Tout.rotation()<<endl;
            }
            N_TRIALS++;
            logger <<"-------------------------------------------\n";
        }
    }
    logger2<<"\n";
    logger2.flush();

}
//}}}}}

cout<<"Trials:  "<<N_TRIALS<<endl;
cout<<"Success: "<<N_SUCC<<"  Rate = "<<(double)N_SUCC/(double)N_TRIALS<<endl;
cout<<"Success + Acc : "<<N_TRIALS-N_FAIL<<"  Rate = "<<1-(double)N_FAIL/(double)N_TRIALS<<endl;
cout<<"Fail:    "<<N_FAIL<<endl;
cout<<"Runtime  "<<C_TIME/N_TRIALS<<endl;

logger2<<"Trials:  "<<N_TRIALS<<endl;
logger2<<"Success: "<<N_SUCC<<"  Rate = "<<(double)N_SUCC/(double)N_TRIALS<<endl;
logger2<<"Success + Acc : "<<N_TRIALS-N_FAIL<<"  Rate = "<<1-(double)N_FAIL/(double)N_TRIALS<<endl;
logger2<<"Fail:    "<<N_FAIL<<endl;
logger2<<"Runtime  "<<C_TIME/N_TRIALS<<endl;

logger2<<"Times = [";
for(int i=0; i<times.size(); i++)
{
    logger2<<times[i]<<" ";
}
logger2<<"];\n";

logger2<<"ER = [";
for(int i=0; i<er.size(); i++)
{
    logger2<<er[i]<<" ";
}
logger2<<"];\n";

logger2<<"ET = [";
for(int i=0; i<et.size(); i++)
{
    logger2<<et[i]<<" ";
}
logger2<<"];\n";

logger2<<"Succ = [";
for(int i=0; i<success.size(); i++)
{
    logger2<<success[i]<<" ";
}
logger2<<"];\n";

return (0);
#endif
}
#endif


// Copyright (C) 2013  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/python.h>
#include <boost/shared_ptr.hpp>
#include <dlib/matrix.h>
#include <dlib/data_io.h>
#include <dlib/sparse_vector.h>
#include <boost/python/args.hpp>
#include <dlib/optimization.h>

using namespace dlib;
using namespace std;
using namespace boost::python;

typedef std::vector<std::pair<unsigned long,double> > sparse_vect;

void _make_sparse_vector (
    sparse_vect& v
)
{
    make_sparse_vector_inplace(v);
}

void _make_sparse_vector2 (
    std::vector<sparse_vect>& v
)
{
    for (unsigned long i = 0; i < v.size(); ++i)
        make_sparse_vector_inplace(v[i]);
}

tuple _load_libsvm_formatted_data (
    const std::string& file_name
)
{
    std::vector<sparse_vect> samples;
    std::vector<double> labels;
    load_libsvm_formatted_data(file_name, samples, labels);
    return make_tuple(samples, labels);
}

void _save_libsvm_formatted_data (
    const std::string& file_name,
    const std::vector<sparse_vect>& samples,
    const std::vector<double>& labels
)
{
    pyassert(samples.size() == labels.size(), "Invalid inputs");
    save_libsvm_formatted_data(file_name, samples, labels);
}

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

list _max_cost_assignment (
    const matrix<double>& cost
)
{
    // max_cost_assignment() only works with integer matrices, so convert from
    // double to integer.
    const double scale = (std::numeric_limits<dlib::int64>::max()/1000)/max(abs(cost));
    matrix<dlib::int64> int_cost = matrix_cast<dlib::int64>(round(cost*scale));
    return vector_to_python_list(max_cost_assignment(int_cost));
}

double _assignment_cost (
    const matrix<double>& cost,
    const list& assignment
)
{
    return assignment_cost(cost, python_list_to_vector<long>(assignment));
}

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

void bind_other()
{
    using boost::python::arg;

    def("max_cost_assignment", _max_cost_assignment, (arg("cost")),
"requires    \n\
    - cost.nr() == cost.nc()    \n\
      (i.e. the input must be a square matrix)    \n\
ensures    \n\
    - Finds and returns the solution to the following optimization problem:    \n\
    \n\
        Maximize: f(A) == assignment_cost(cost, A)    \n\
        Subject to the following constraints:    \n\
            - The elements of A are unique. That is, there aren't any     \n\
              elements of A which are equal.      \n\
            - len(A) == cost.nr()    \n\
    \n\
    - Note that this function converts the input cost matrix into a 64bit fixed    \n\
      point representation.  Therefore, you should make sure that the values in    \n\
      your cost matrix can be accurately represented by 64bit fixed point values.    \n\
      If this is not the case then the solution my become inaccurate due to    \n\
      rounding error.  In general, this function will work properly when the ratio    \n\
      of the largest to the smallest value in cost is no more than about 1e16.   "
        );

    def("assignment_cost", _assignment_cost, (arg("cost"),arg("assignment")),
"requires    \n\
    - cost.nr() == cost.nc()    \n\
      (i.e. the input must be a square matrix)    \n\
    - for all valid i:    \n\
        - 0 <= assignment[i] < cost.nr()    \n\
ensures    \n\
    - Interprets cost as a cost assignment matrix. That is, cost[i][j]     \n\
      represents the cost of assigning i to j.      \n\
    - Interprets assignment as a particular set of assignments. That is,    \n\
      i is assigned to assignment[i].    \n\
    - returns the cost of the given assignment. That is, returns    \n\
      a number which is:    \n\
        sum over i: cost[i][assignment[i]]   "
        );

    def("make_sparse_vector", _make_sparse_vector ,
"This function modifies its argument so that it is a properly sorted sparse vector.    \n\
This means that the elements of the sparse vector will be ordered so that pairs    \n\
with smaller indices come first.  Additionally, there won't be any pairs with    \n\
identical indices.  If such pairs were present in the input sparse vector then    \n\
their values will be added together and only one pair with their index will be    \n\
present in the output.   "
        );
    def("make_sparse_vector", _make_sparse_vector2 ,
        "This function modifies a sparse_vectors object so that all elements it contains are properly sorted sparse vectors.");

    def("load_libsvm_formatted_data",_load_libsvm_formatted_data, (arg("file_name")),
"ensures    \n\
    - Attempts to read a file of the given name that should contain libsvm    \n\
      formatted data.  The data is returned as a tuple where the first tuple    \n\
      element is an array of sparse vectors and the second element is an array of    \n\
      labels.    "
    );

    def("save_libsvm_formatted_data",_save_libsvm_formatted_data, (arg("file_name"), arg("samples"), arg("labels")),
"requires    \n\
    - len(samples) == len(labels)    \n\
ensures    \n\
    - saves the data to the given file in libsvm format   "
    );
}


//  test_special_values.cpp  ----------------------------------------------------------//

//  Copyright 2008 Howard Hinnant
//  Copyright 2008 Beman Dawes
//  Copyright 2009 Vicente J. Botet Escriba

//  Distributed under the Boost Software License, Version 1.0.
//  See http://www.boost.org/LICENSE_1_0.txt

/*
This code was extracted by Vicente J. Botet Escriba from Beman Dawes time2_demo.cpp which
was derived by Beman Dawes from Howard Hinnant's time2_demo prototype.
Many thanks to Howard for making his code available under the Boost license.
The original code was modified to conform to Boost conventions and to section
20.9 Time utilities [time] of the C++ committee's working paper N2798.
See http://www.open-std.org/jtc1/sc22/wg21/docs/papers/2008/n2798.pdf.

time2_demo contained this comment:

    Much thanks to Andrei Alexandrescu,
                   Walter Brown,
                   Peter Dimov,
                   Jeff Garland,
                   Terry Golubiewski,
                   Daniel Krugler,
                   Anthony Williams.
*/

#include <boost/chrono/chrono.hpp>
#include <boost/type_traits.hpp>

#include <iostream>

using namespace boost::chrono;

void test_special_values()
{
    std::cout << "duration<unsigned>::min().count()  = " << ((duration<unsigned>::min)()).count() << '\n';
    std::cout << "duration<unsigned>::zero().count() = " << duration<unsigned>::zero().count() << '\n';
    std::cout << "duration<unsigned>::max().count()  = " << ((duration<unsigned>::max)()).count() << '\n';
    std::cout << "duration<int>::min().count()       = " << ((duration<int>::min)()).count() << '\n';
    std::cout << "duration<int>::zero().count()      = " << duration<int>::zero().count() << '\n';
    std::cout << "duration<int>::max().count()       = " << ((duration<int>::max)()).count() << '\n';
}

int main()
{
    test_special_values();
    return 0;
}


#include <boost/proto/debug.hpp>
#include <boost/test/unit_test.hpp>
#include <string>

#include <metaSMT/frontend/Logic.hpp>
#include <metaSMT/frontend/QF_BV.hpp>
#include <metaSMT/frontend/QF_UF.hpp>
#include <metaSMT/API/SymbolTable.hpp>

#include <metaSMT/expression/expression.hpp>

using namespace std;
using namespace metaSMT;
using namespace metaSMT::solver;
using namespace metaSMT::logic;
using namespace metaSMT::logic::QF_BV;
using namespace metaSMT::logic::QF_UF;
using namespace metaSMT::expression;
namespace proto = boost::proto;
using boost::dynamic_bitset;

BOOST_FIXTURE_TEST_SUITE(expression_t, Solver_Fixture )

BOOST_AUTO_TEST_CASE( expression_equality ) {
  logic_expression b0 = bit0_const();
  logic_expression b1 = bit1_const();
  BOOST_REQUIRE( b0 == b0 );
  BOOST_REQUIRE( !(b0 == b1) );
  BOOST_REQUIRE( b1 == b1 );
  BOOST_REQUIRE( !(b1 == b0) );

  logic_expression p = new_variable();
  BOOST_REQUIRE( p == p );

  logic_expression q = new_variable();
  BOOST_REQUIRE( !(p == q) );
  BOOST_REQUIRE( !(q == p) );

  logic_expression bv_a = new_bitvector();
  BOOST_REQUIRE( bv_a == bv_a );
  BOOST_REQUIRE( !(p == bv_a) );

  logic_expression bv_b = new_bitvector();
  BOOST_REQUIRE( !(bv_a == bv_b) );
  BOOST_REQUIRE( !(bv_b == bv_a) );

  logic_expression and1_expr = evaluate(ctx, And(p, q));
  logic_expression and2_expr = evaluate(ctx, And(p, q));
  BOOST_REQUIRE( and1_expr == and2_expr );

  logic_expression func1 = declare_function(Boolean())(BitVector(8));
  logic_expression func2 = declare_function(Boolean())(BitVector(6));
  BOOST_REQUIRE( func1 == func1 );
  BOOST_REQUIRE( !(func1 == func2) );
}

BOOST_AUTO_TEST_SUITE_END() //Solver

//  vim: ft=cpp:ts=2:sw=2:expandtab


/*
Copyright (c) 2013, Sibt ul Hussain <sibt.ul.hussain at gmail dot com>
All rights reserved.
Released under BSD License
-------------------------
For license terms please see license.lic
*/
//#include <boost/bind.hpp>
#include<vector>
#include<Magick++.h>
#include<dirent.h>
#include<iostream>
#include<string>
#include<cmath>
#include<cstdio>
#include<fstream>
#include <cstdlib>
#include<algorithm>
#include<numeric>
#include <sstream>
#include<set>
#include"Sorter.h"
#include <boost/thread.hpp>
#include <boost/program_options.hpp>
#ifdef WITH_BLITZ
#include <blitz/array.h>
#include <blitz/tinyvec.h>
#include <blitz/tinymat.h>
#include <blitz/tinyvec-et.h>
#endif
//#include "../Eigen/QR"
//using namespace Eigen;

//#include "../Eigen/Core"
//#include "../Eigen/Array"
//#define EIGEN2_SUPPORT
#define EIGEN_DEFAULT_TO_ROW_MAJOR // to use row major order for eigen class
#include "../Eigen/Core"
#include "../Eigen/Dense"
using namespace std;
using namespace Eigen;

#include "bips.h"

#ifndef UTIL_H_
#define UTIL_H_
using namespace Magick;
typedef unsigned int UINT;
typedef float REAL;
typedef unsigned char UCHAR;
#ifdef WITH_BLITZ
typedef blitz::Array<REAL, 2> Array2DReal;
#endif
enum FilterType {
	Gaussian, Bilinear
};
enum DistanceFunction {
	DF_NormalizedIntersection, DF_CHISquared, DF_L2,
};
#ifndef MIN
#define MIN(a,b) ( ( (a) < (b) ) ? (a) : (b) )
#endif

#ifndef MAX
#define MAX(a,b) ( ( (a) > (b) ) ? (a) : (b) )
#endif
// last three signifies the Opponent ColorSpace....
enum Channel {
	Red = 0,
	Green,
	Blue,
	Gray,
	RGB,
	O1,
	O2,
	O3,
	HSL,
	LAB,
	WandellOCS,
	SandeOCS,
	ABSGradientMag
};
// Where O1== luminance component...
// O2 is the red-green channel
// O3 is the blue-yellow channel....

enum ClassifierType {
	SVMLIGHT,
	L1LR,
	L2SVM_L1Reg,
	L1SVM_L2Reg,
	LatentSVM,
	PLSQDA,
	GaussianSVM,
	QuadEmbedSVM,
	LinearEmbedSVM,
	CubicEmbedSVM,
	IKSVM,
	LibLinearPhi2,
	L2LR,
	LinQuadEmbed,
	NormalIKSVM,
	PolyEmbedSVM,
	CT_RandomForests,
	CT_SMIDAS,
	LIBOCAS
};

#ifndef ABS
#define ABS(x) ( ( (x) < 0 ) ?  -(x)   : (x) )
#endif

static string GetClassifierType(const ClassifierType& ctype) {
	string val[] = { "SVMLIGHT", "L1LR", "L2SVM_L1Reg", "L1SVM_L2Reg",
			"LatentSVM", "PLSQDA", "GaussianSVM", "QuadEmbedSVM",
			"LinearEmbedSVM", "CubicEmbedSVM", "IKSVM", "LibLinearPhi2",
			"L2LR", "LinQuadEmbed", "NormalIKSVM", "PolyEmbedSVM",
			"CT_RandomForests" };
	return val[MIN(ctype,16)];
}

static string GetChannelName(Channel ctype) {
	const int nchn = 13;
	string chnames[] = { "Red", "Green", "Blue", "Gray", "RGB", "O1", "O2",
			"O3", "HSL", "LAB", "WandellOCS", "SandeOCS", "ABSGradientMag" };
	return chnames[MIN(nchn,ctype)];
}
void ReadDir(const string &dname, const string &, const string &ofname);
void ReadDir(const string &dname, const string &ofname);
void ReadDir(const string dname, vector<string> &lname);
void ReadImage(unsigned char *mat, const Image & image, unsigned int offset);
void ReadImage(const Image &image, vector<REAL> &pixval, REAL = 1);
void ReadColorImage(const Image&image, vector<REAL>&pixval, int = 1);
void Scale(Image &simage, UINT rows, UINT cols, bool aspect);
void Scale(Image &simage, REAL scale);
string int2str(int input);
void CopyBoundaries(Image& input, Image& output, char dir);
void Padding(Image& input, Image& output, UINT size, char dir);

void ResizeImage(Image & image, UINT width, UINT height, FilterType = Bilinear,
		bool = false);

void Reflect(Image& input, Image& output, UINT size, char dir);
void ReflectImage(Image& output, UINT size, char dir);
void BilinearInterpolation(vector<float>& input, int cols, int rows,
		float scale, vector<float>& output);
void BilinearInterpolation(vector<REAL>& input, UINT cols, UINT rows, UINT dim,
		REAL scale, vector<REAL>& output);
void Compare(const string& nfile1, const string& nfile2, vector<string>&diff);
void ReadFile(const string &nfile, vector<string>&vstr);

// Modified version of BinarySearch gives the closest element's
// index to the element being searched...
//
//static int BinarySearch(vector<BoostInfo> &data, int size, REAL searchElement) {
//	//	int size = data.size();
//	int low = 0; // low end of the search area
//	int high = size - 1; // high end of the search area
//	int middle = (low + high + 1) / 2; // middle element
//	int location = -1; // return value; -1 if not found
//
//	if (searchElement >= data[high].weight)
//		location = high;
//	else if (searchElement <= data[low].weight)
//		location = low;
//	else {
//		do // loop to search for element
//		{
//			// print remaining elements of vector to be searched
//			//				displaySubElements(data, low, high);
//
//			// output spaces for alignment
//			for (int i = 0; i < middle; i++)
//				cout << "   ";
//
//			//				cout << " * " << endl; // indicate current middle
//
//			// if the element is found at the middle
//			if (searchElement == data[middle].weight)
//				location = middle; // location is the current middle
//			else if (searchElement < data[middle].weight) // middle is too high
//				high = middle - 1; // eliminate the higher half
//			else
//				// middle element is too low
//				low = middle + 1; // eliminate the lower half
//
//			middle = (low + high + 1) / 2; // recalculate the middle
//		} while ((low <= high) && (location == -1));
//
//		if (location == -1) // no elements in the list, so return the next element in the ordered list.
//			location = MIN(low, size - 1);
//	}
//
//	return location; // return location of search key
//}

// Comparison Functors...
struct cmpInts {
	bool operator ()(int i1, int i2) {
		return i1 < i2;
	}
};
template<class T>
struct Cmp {
	bool operator ()(const T &i1, const T &i2) {
		return i1 < i2;
	}
};

template<class T>
struct CmpAbsDescend {
	bool operator ()(const T &i1, const T &i2) {
		return ABS(i1) > ABS(i2);
	}
};
template<class T> // utility function to display
ostream& operator<<(ostream& os, const vector<T>& v) {
	copy(v.begin(), v.end(), ostream_iterator<T> (cout, " "));
	return os;
}
template<class T>
void Print(T *start, T *end) {
	cout << endl;
	for (T *p = start; p < end; ++p)
		cout << *p << " ";
	cout << endl;
}
template<class T> // utility function to display
ofstream& operator<<(ofstream& os, const vector<T>& v) {
	copy(v.begin(), v.end(), ostream_iterator<T> (os, " "));
	return os;
}
template<class T>
void Print(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &mMatrix) {

	for (UINT i = 0; i < mMatrix.rows(); ++i) {
		for (UINT j = 0; j < mMatrix.cols(); ++j)
			cout << mMatrix(i, j) << " ";
		cout << endl;
	}

}
template<class T>
void WriteMatrix2File(const string &fname,
		Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &mMatrix) {

	ofstream ofile(fname.c_str(), ios::out);
	if (!ofile) {
		cout << " Couldn't Create the File " << fname << endl;
		return;
	}
	for (UINT i = 0; i < mMatrix.rows(); ++i) {
		for (UINT j = 0; j < mMatrix.cols(); ++j)
			ofile << mMatrix(i, j) << " ";
		ofile << endl;
	}
	ofile.close();
}
template<class T>
void WriteMatrix2File(const string &fname, Eigen::Map<T> &mMatrix) {

	ofstream ofile(fname.c_str(), ios::out);
	if (!ofile) {
		cout << " Couldn't Create the File " << fname << endl;
		return;
	}
	for (UINT i = 0; i < mMatrix.rows(); ++i) {
		for (UINT j = 0; j < mMatrix.cols(); ++j)
			ofile << mMatrix(i, j) << " ";
		ofile << endl;
	}
	ofile.close();
}
template<class T>
void Print(Eigen::Map<T> mMatrix) {

	for (UINT i = 0; i < mMatrix.rows(); ++i) {
		for (UINT j = 0; j < mMatrix.cols(); ++j)
			cout << mMatrix(i, j) << " ";
		cout << endl;
	}

}
template<class T>
void RandomPermute(vector<T> &array) {
	int n = array.size(); // The number of items left to shuffle (loop invariant).
	while (n > 1) {
		int k = rand() % n; // 0 <= k < n.
		n--; // n is now the last pertinent index;
		T temp = array[n]; // swap array[n] with array[k] (does nothing if k == n).
		array[n] = array[k];
		array[k] = temp;
	}
}
template<class T>
void RandomPermute(vector<T> &array, UINT limit)
// Samples the indeces from a given limit...
{
	int n;
	if (limit >= array.size()) {
		n = array.size();
	} else
		n = limit; // The number of items left to shuffle (loop invariant).
	while (n > 1) {
		int k = rand() % n; // 0 <= k < n.
		n--; // n is now the last pertinent index;
		T temp = array[n]; // swap array[n] with array[k] (does nothing if k == n).
		array[n] = array[k];
		array[k] = temp;
	}
}
void DrawRectangle(Image &image, int startx, int starty, int width, int height,
		const string &col);

void CropImage(Image &image, int xmin, int ymin, int width, int height);
void CropImage2(Image &image, int xmin, int ymin, int xmax, int ymax);
void PrintErrorExit(string msg);
void ParseListFile(const string&, vector<string>&);
void Dec2Bin(UINT, string&);
string SplitFilenameFromDir(const string& str);

//template <class T>
//UINT Bin2Dec(T *ptr,const UINT &size)
//{
//	UINT val = 0, count = 0;
//	for (UINT k = size;k >= 0; --k, ++count)
//		val += (UINT) pow(2.0, (double) count) * ptr[k];
//	return val;
//
//}
UINT Bin2Dec(const string&);
//template <class T>
//T FindMaximum(vector<T>& ivec,UINT &index);
//{
//	index=0;
//	T maxval=ivec[0];
//	UINT k=0;
////	for(vector<T>::iterator iter=ivec.begin();;)
////	for(vector<T>::iterator iter=ivec.begin(); iter!=ivec.end();++iter,++k)
//	for(int k=0; k < ivec.size();++k)
//		if( ivec[k] > maxval)
//		{
//			index = k;
//			maxval = ivec[k];
//		}
//	return maxval;
//}
// Read Matlab Matrix
#ifdef WITH_BLITZ
Array2DReal ReadData(const string &fname);
void Write2File(const string & ofname, const Array2DReal & flowu);
#endif
string SplitFilename(const string& str);
//{
//	std::ofstream ofile(ofname.c_str());
//	blitz::TinyVector<int, 2> extent = flowu.extent();
//	for (int i = 0; i < extent[0]; ++i) {
//		for (int j = 0; j < extent[1]; ++j) {
//			ofile << flowu(i, j) << " ";
//		}
//		ofile << endl;
//	}
//	ofile.close();
//}
unsigned long getActiveMemory();
UINT GetNumberLines(const string & fname);
void fwrite_u8_pnm(const char* file, int nx, int ny, int nc,
		const u8 im[/*0:nx-1@dx,0:ny-1@dy,0:nc-1@dc*/], int dx, int dy, int dc);
void CopyConstBoundaries(Image& input, UINT size);
template<class T>
void WriteVector2File(const string& fname, T * feat, int length) {
	ofstream ofile(fname.c_str(), ios::out);
	if (!ofile) {
		cout << " Couldn't Create the File " << fname << endl;
		return;
	}
	for (int i = 0; i < length; ++i)
		ofile << feat[i] << " ";
	ofile << endl;
	ofile.close();
}
template<class T>
void WriteVector2File(ofstream &ofile, T * feat, int length) {
	for (int i = 0; i < length; ++i)
		ofile << feat[i] << " ";
	ofile << endl;
	//	ofile.close();
}
template<class T>
void WriteVector2File(const string &fname, vector<T> &feat) {
	int length = feat.size();
	ofstream ofile(fname.c_str(), ios::out | ios::binary);
	if (!ofile) {
		cout << " Couldn't Create the File " << fname << endl;
		return;
	}
	ofile.write((char*) &length, sizeof(UINT));
	ofile.write((char*) (&feat[0]), sizeof(T) * length);
	ofile.close();
}
void WriteStrings2File(const string &fname, vector<string> &feat);
template<class T>
void WriteMatrix2File(const string& fname, T **feat, const UINT &rows,
		const UINT &cols) {
	ofstream ofile(fname.c_str(), ios::out);
	if (!ofile) {
		cout << " Couldn't Create the File " << fname << endl;
		return;
	}
	for (UINT i = 0; i < rows; ++i) {
		for (UINT j = 0; j < cols; ++j)
			ofile << feat[i][j] << " ";
		ofile << endl;
	}

	ofile.close();
}
template<class T>
void WriteImageFile(const string& fname, vector<T>& r, vector<T>&g,
		vector<T>&b, UINT rows, UINT cols) {
	ofstream ofile(fname.c_str(), ios::out);
	if (!ofile) {
		cout << " Couldn't Create the File " << fname << endl;
		return;
	}
	ofile.write((char*) &rows, sizeof(UINT));
	ofile.write((char*) &cols, sizeof(UINT));
	ofile.write((char*) &r[0], sizeof(T) * (rows * cols));
	ofile.write((char*) &g[0], sizeof(T) * (rows * cols));
	ofile.write((char*) &b[0], sizeof(T) * (rows * cols));
	ofile.close();
}
template<class T>
void WriteMatrix2File(const string& fname, vector<vector<T> >&feat) {
	ofstream ofile(fname.c_str(), ios::out);
	if (!ofile) {
		cout << " Couldn't Create the File " << fname << endl;
		return;
	}
	for (UINT i = 0; i < feat.size(); ++i) {
		for (UINT j = 0; j < feat[i].size(); ++j)
			ofile << feat[i][j] << " ";
		ofile << endl;
	}

	ofile.close();
}
//void WriteRealMatrix2BinFile(const string& fname, vector<vector<REAL> >&feat);
//void ReadRealMatrixFromBinFile(const string& fname, vector<vector<REAL> >&feat);
//void WriteRealMatrix2BinFile(const string& fname, vector<vector<REAL> >&feat,
//		const vector<string>&);
//void ReadRealMatrixFromBinFile(const string& fname, vector<vector<REAL> >&feat,
//		vector<string>&);

template<class T>
void WriteRealMatrix2BinFile(const string& fname, vector<vector<T> >&feat) {
	ofstream ofile(fname.c_str(), ios::out | ios::binary);
	if (!ofile) {
		cout << " Couldn't Create the File " << fname << endl;
		return;
	}
	UINT nfeat = feat.size(), featdim = feat[0].size();
	ofile.write((char*) &nfeat, sizeof(UINT));
	ofile.write((char*) &featdim, sizeof(UINT));
	for (UINT i = 0; i < feat.size(); ++i) {
		ofile.write((char*) (&feat[i][0]), sizeof(T) * featdim);
	}
	ofile.close();
}
template<class T>
void WriteRealMatrix2BinFile(const string& fname, vector<vector<T> >&feat,
		const vector<string>&labels) {
	assert(feat.size() == labels.size());
	ofstream ofile(fname.c_str(), ios::out | ios::binary);
	if (!ofile) {
		cout << " Couldn't Create the File " << fname << endl;
		return;
	}
	UINT nfeat = feat.size(), featdim = feat[0].size();
	ofile.write((char*) &nfeat, sizeof(UINT));
	ofile.write((char*) &featdim, sizeof(UINT));
	for (UINT i = 0; i < feat.size(); ++i) {
		UINT tvar = labels[i].length();
		ofile.write((char*) &tvar, sizeof(UINT));
		ofile.write((char*) labels[i].c_str(), sizeof(char) * tvar);
		ofile.write((char*) (&feat[i][0]), sizeof(T) * featdim);
	}
	ofile.close();
}
template<class T>
void ReadRealMatrixFromBinFile(const string& fname, vector<vector<T> >&feat) {
	ifstream ifile(fname.c_str(), ios::in | ios::binary);
	if (!ifile) {
		cout << " Couldn't Create the File " << fname << endl;
		return;
	}
	UINT nfeat, featdim;
	ifile.read((char*) &nfeat, sizeof(UINT));
	ifile.read((char*) &featdim, sizeof(UINT));
	feat.resize(nfeat, vector<T> (featdim, 0));
	for (UINT i = 0; i < feat.size(); ++i) {
		ifile.read((char*) (&feat[i][0]), sizeof(T) * featdim);
	}

	ifile.close();
}
template<class T>
void ReadRealMatrixFromBinFile(const string& fname, vector<vector<T> >&feat,
		vector<string>&labels) {
	ifstream ifile(fname.c_str(), ios::in | ios::binary);
	if (!ifile) {
		cout << " Couldn't Create the File " << fname << endl;
		return;
	}
	UINT nfeat, featdim;
	ifile.read((char*) &nfeat, sizeof(UINT));
	ifile.read((char*) &featdim, sizeof(UINT));
	feat.resize(nfeat, vector<T> (featdim, 0));
	char lab[1024];
	for (UINT i = 0; i < feat.size(); ++i) {
		UINT tvar;
		ifile.read((char*) &tvar, sizeof(UINT));
		ifile.read((char*) lab, sizeof(char) * tvar);
		labels.push_back(lab);
		ifile.read((char*) (&feat[i][0]), sizeof(T) * featdim);
	}
	ifile.close();
}

template<class T>
void WriteMatrix2File(const string& fname, T *feat, const UINT &rows,
		const UINT &cols) {
	ofstream ofile(fname.c_str(), ios::out);
	if (!ofile) {
		cout << " Couldn't Create the File " << fname << endl;
		return;
	}
	T *ptr = feat;
	for (UINT i = 0; i < rows; ++i) {
		for (UINT j = 0; j < cols; ++j)
			ofile << ptr[j] << " ";
		ofile << endl;
		ptr += cols;
	}

	ofile.close();
}
template<class T>
T Dot(vector<T>& feat) {
	T sum = 0;
	for (typename vector<T>::iterator iter = feat.begin(); iter != feat.end(); ++iter)
		sum += *iter * *iter;
	return sum;
}
template<class T> /*Can Use inner_product instead*/
T Dot(vector<T>& feat, vector<T>&filter) {
	T sum = 0;
	for (typename vector<T>::iterator iter = feat.begin(), fiter =
			filter.begin(); iter != feat.end(); ++iter, ++fiter)
		sum += *iter * *fiter;
	return sum;
}
template<class T> /*Can Use inner_product instead*/
T Dot(vector<T>& feat, T *filter) {
	T sum = 0;
	T *ptr = filter;
	for (typename vector<T>::iterator iter = feat.begin(); iter != feat.end(); ++iter, ++ptr)
		sum += *iter * *ptr;
	return sum;
}
template<class T>
void ReadTextFile(const string &fname, vector<vector<T> >&vec, UINT &nexamples,
		UINT &dimfeature) {
	ifstream ifile(fname.c_str(), ios::in);
	if (!ifile) {
		cout << endl << " Error Couldn't Load the file " << fname << endl;
		exit(EXIT_FAILURE);
	}
	ifile >> nexamples >> dimfeature;

	vec.resize(nexamples, vector<T> (dimfeature, 0));

	for (UINT i = 0; i < nexamples; ++i) {

		for (UINT j = 0; j < dimfeature && ifile; ++j)
			ifile >> vec[i][j];

	}
}
template<class T>
void ReadTextFile(const string &fname, T **&vec, UINT &nexamples,
		UINT &dimfeature) {
	ifstream ifile(fname.c_str(), ios::in);
	if (!ifile) {
		cout << endl << " Error Couldn't Load the file " << fname << endl;
		exit(EXIT_FAILURE);
	}
	ifile >> nexamples >> dimfeature;

	vec = new T*[nexamples];

	for (UINT i = 0; i < nexamples; ++i) {
		vec[i] = new T[dimfeature];

		for (UINT j = 0; j < dimfeature && ifile; ++j)
			ifile >> vec[i][j];
	}

}
template<class T>
void ReadTextFile(const string &fname, T *&vec, UINT &nexamples,
		UINT &dimfeature) {
	ifstream ifile(fname.c_str(), ios::in);
	if (!ifile) {
		cout << endl << " Error Couldn't Load the file " << fname << endl;
		exit(EXIT_FAILURE);
	}
	ifile >> nexamples >> dimfeature;

	vec = new T[nexamples * dimfeature];
	REAL *tvec = vec;
	for (UINT i = 0; i < nexamples * dimfeature; ++i, ++tvec) {
		ifile >> *tvec;
	}
	ifile.close();
}
template<class T>
void ReadFile(const string &fname, T **&vec, UINT &nexamples, UINT &dimfeature) {
	ifstream ifile(fname.c_str(), ios::in | ios::binary);
	if (!ifile) {
		cout << endl << " Error Couldn't Load the file " << fname << endl;
		exit(EXIT_FAILURE);
	}
	ifile.read((char*) &nexamples, sizeof(UINT));
	ifile.read((char*) &dimfeature, sizeof(UINT));
	cout << "\n File  " << fname << " has " << nexamples
			<< " Examples  with Dimension =" << dimfeature << endl;

	vec = new T*[nexamples];

	for (UINT i = 0; i < nexamples; ++i) {
		vec[i] = new T[dimfeature];

		ifile.read((char*) vec[i], sizeof(REAL) * dimfeature);
	}
	ifile.close();
}
template<class T>
void ReadFile(const string &fname, vector<vector<T> > &vec) {
	ifstream ifile(fname.c_str(), ios::in | ios::binary);
	if (!ifile) {
		cout << endl << " Error Couldn't Load the file " << fname << endl;
		exit(EXIT_FAILURE);
	}
	UINT nexamples, dimfeature;
	ifile.read((char*) &nexamples, sizeof(UINT));
	ifile.read((char*) &dimfeature, sizeof(UINT));
	cout << "\n File  " << fname << " has " << nexamples
			<< " Examples  with Dimension =" << dimfeature << endl;

	vec.resize(nexamples, vector<T> (dimfeature, 0));

	for (UINT i = 0; i < nexamples; ++i) {
		T *vecptr = &vec[i][0];
		ifile.read((char*) vecptr, sizeof(REAL) * dimfeature);
	}
	ifile.close();
}
void FindCrossValidationIndeces(UINT kfold, UINT tnexamples,
		vector<vector<UINT> >& trainidx, vector<vector<UINT> >& testidx);

template<class T>
UINT Bin2Dec(T *ptr, const UINT &size) {
	UINT val = 0, count = size - 1;
	for (UINT k = 0; k < size; ++k, --count)
		val += (UINT) pow(2.0, (double) count) * ptr[k];
	return val;
}
template<class Template>
void WriteStorage(ofstream &ofile, Template &tem) {
	long m = tem.rows(), n = tem.cols();
	ofile.write((char*) &m, sizeof(long));
	ofile.write((char*) &n, sizeof(long));
	double val;
	for (long i = 0; i < m; ++i)
		for (long j = 0; j < n; ++j) {
			val = tem(i, j);
			ofile.write((char*) &val, sizeof(double));
		}

}
template<class Template>
void ReadStorage(ifstream &ifile, Template &tem) {
	long m, n;
	ifile.read((char*) &m, sizeof(long));
	ifile.read((char*) &n, sizeof(long));
	if (m > 0 && n > 0) {
		tem = Template::Zero(m, n);
		double val;
		for (long i = 0; i < m; ++i)
			for (long j = 0; j < n; ++j) {
				ifile.read((char*) &val, sizeof(double));
				tem(i, j) = val;
			}
	}
}
template<class T1, class T2>
map<T1, T2> AddMaps(map<T1, T2> & m1, map<T1, T2> &m2) {
	map<T1, T2> tobj;
	for (typename map<T1, T2>::iterator p = m1.begin(); (p != m1.end()); ++p)
		tobj[p->first] = p->second;

	for (typename map<T1, T2>::iterator p = m2.begin(); (p != m2.end()); ++p)
		tobj[p->first] = tobj[p->first] + p->second;

	return tobj;
}
template<class T1, class T2>
void DisplayMaps(const map<T1, T2> & m1) {

	cout << "\n-------------------------------------------\n";
	for (typename map<T1, T2>::const_iterator p = m1.begin(); (p != m1.end()); ++p)
		cout << "Key = " << p->first << ", Value =" << p->second << ";" << endl;

}
bool CanReadFile(const string &fname);
//////////////////////////////////////////////////////////////////////////////
//
// process_mem_usage(double &, double &) - takes two doubles by reference,
// attempts to read the system-dependent data for a process' virtual memory
// size and resident set size, and return the results in KB.
//
// On failure, returns 0.0, 0.0

void ProcessMemoryUsage(double& vm_usage, double& resident_set);
void Split(int *start, int *end, int *&ls, int *&le, int *&rs, int *&re,
		int threshold);
void DisplayMemoryUsage();
template<class T, class C>
REAL L2Dist(vector<T> & vec1, vector<C> & vec2) {

	REAL dist = 0;
	typename vector<T>::iterator v1 = vec1.begin();
	typename vector<C>::iterator v2 = vec2.begin();
	for (; v1 != vec1.end(); ++v1, ++v2) {
		REAL d = (*v1 - *v2);
		dist += d * d;
	}
	return sqrt(dist);
}
template<class T, class C>
REAL L2Dist(T * vec1, C*vec2, UINT size) {

	REAL dist = 0;
	T *v1 = vec1;
	C *v2 = vec2;
	for (; v1 != vec1 + size; ++v1, ++v2) {
		REAL d = (*v1 - *v2);
		dist += d * d;
	}
	return sqrt(dist);
}
void MyGaussianFilter(REAL sigma, vector<REAL> &filter);
void PadImage(Image& out, UINT dim[]);
void GetCrossValidationIndeces(UINT kfold, UINT npos, UINT nneg,
		vector<vector<UINT> >&trainidx, vector<vector<UINT> >&testidx);
void PrintIdx(vector<UINT>&trainidx, vector<UINT>&testidx,
		UINT ptrainoffset = 0, UINT ptestoffset = 0);
template<class T>
void Convolve(vector<T> & data, vector<T>&filter) {
	UINT hfsize = filter.size() / 2;
	if (data.size() < filter.size())
		return;
	UINT dsize = data.size();
	vector<REAL> response(dsize + 2 * hfsize, 0);
	copy(data.begin(), data.end(), response.begin() + hfsize);
	// central part
	fill(data.begin(), data.end(), 0);
	for (UINT i = hfsize, count = 0; i < response.size() - hfsize; ++i, count++) {
		UINT j = count; // i-hfsize
		for (typename vector<T>::iterator iter = filter.begin(); iter
				!= filter.end(); ++iter, ++j) {
			data[count] += *iter * response[j];
		}
	}

}

template<class T>
void DisplayVector(vector<T>& data) {
	cout << endl;
	UINT count = 0;
	for (typename vector<T>::iterator iter = data.begin(); iter != data.end(); ++iter) {
		cout << *iter << " , ";
		++count;
		if (count % 10 == 0)
			cout << endl;
	}
	cout << endl;
}
template<typename T>
std::string ToString(const T& x) {
	std::ostringstream ss;
	ss << x;
	return ss.str();
}
string strtokenize(const string& str, char ch, bool linstance = false); /// can tokenize based on the last instance of the character, otherwise first instance is used.
void AppendPath(const string &pathdir, vector<string>&fnames);
template<class T>
T FindDistance(vector<T>& feat1, vector<T>& feat2,
		DistanceFunction dftype = DF_L2) {
	double distance = 0;
	double eps = 2.2204e-16;
	assert(feat1.size() == feat2.size());
	typename vector<T>::iterator v1 = feat1.begin();
	typename vector<T>::iterator v2 = feat2.begin();
	switch (dftype) {
	case DF_CHISquared:
		//		for (int i = 0; i < feat1.size(); ++i)
		//			distance += ((feat1[i] - feat2[i]) * (feat1[i] - feat2[i]))
		//					/ (feat1[i] + feat2[i] + 2.2204e-16);

		for (; v1 != feat1.end(); ++v1, ++v2) {
			distance += ((*v1 - *v2) * (*v1 - *v2)) / ((*v1 + *v2) + eps);
			//			distance += d * d;
		}
		distance /= 2;
		break;
	case DF_L2:
		for (; v1 != feat1.end(); ++v1, ++v2) {
			distance += (*v1 - *v2) * (*v1 - *v2);
		}
		// having sqrt doesn't have any impact
		break;
	case DF_NormalizedIntersection:
	default:
		for (; v1 != feat1.end(); ++v1, ++v2)
			distance += MIN( *v1 , *v2);
		distance = -distance;// because it measures the proximity
		break;
	}
	//		cout << endl << feat1 << endl;
	//		cout << feat2 << endl;
	//		cout << distance << endl;
	return distance;
}
template<class T>
T FindDistance(vector<T>& feat1, vector<T>& feat2, DistanceFunction dftype,
		UINT cellsize, vector<REAL>&dist) {
	double distance = 0;
	double eps = 2.2204e-16;
	assert(feat1.size() == feat2.size());
	typename vector<T>::iterator v1 = feat1.begin();
	typename vector<T>::iterator v2 = feat2.begin();
	switch (dftype) {
	case DF_CHISquared:
		for (; v1 != feat1.end(); ++v1, ++v2) {
			distance += ((*v1 - *v2) * (*v1 - *v2)) / ((*v1 + *v2) + eps);
		}
		distance /= 2;
		break;
	case DF_L2:
		for (; v1 != feat1.end(); ++v1, ++v2) {
			distance += (*v1 - *v2) * (*v1 - *v2);
		}
		// its is L2 Square distance
		break;
	case DF_NormalizedIntersection:
	default:
		for (; v1 != feat1.end(); ++v1, ++v2)
			distance += MIN( *v1 , *v2);
		distance = -distance;// because it measures the proximity
		break;
	}
	//		cout << endl << feat1 << endl;
	//		cout << feat2 << endl;
	//		cout << distance << endl;
	return distance;
}
// program_option validator...
void validate(boost::any& v, const std::vector<std::string>& values,
		Channel* target_type, int);
#endif /*UTIL_H_*/


//  Copyright (c) 2015 Boost.Test team
//  Distributed under the Boost Software License, Version 1.0.
//  (See accompanying file LICENSE_1_0.txt or copy at
//  http://www.boost.org/LICENSE_1_0.txt)

//  See http://www.boost.org/libs/test for the library home page.

//[example_code
#define BOOST_TEST_MODULE tolerance_05
#include <boost/test/included/unit_test.hpp>
#include <boost/optional.hpp>
#include <boost/optional/optional_io.hpp>

BOOST_AUTO_TEST_CASE(test, * boost::unit_test::tolerance(0.02))
{
  double                  d1 = 1.00, d2 = 0.99;
  boost::optional<double> o1 = 1.00, o2 = 0.99;

  BOOST_TEST(d1 == d2);   // with tolerance    (double vs. double)
  BOOST_TEST(o1 == o2);   // without tolerance (optional vs. optional)
  BOOST_TEST(o1 == d2);   // without tolerance (optional vs. double)
  BOOST_TEST(*o1 == *o2); // with tolerance    (double vs. double)
}
//]


/*
  (c) Copyright 2008, Hewlett-Packard Development Company, LP

  See the file named COPYING for license details
*/

#ifndef TMP_TUPLES_HPP
#define TMP_TUPLES_HPP

#include <bitset>

#include <boost/tuple/tuple_comparison.hpp>
#include <boost/tuple/tuple_io.hpp>

#include <Lintel/HashTable.hpp>

namespace boost { namespace tuples {
        inline uint32_t hash(const null_type &) { return 0; }
        inline uint32_t hash(const bool a) { return a; }
        inline uint32_t hash(const int32_t a) { return a; }
        inline uint64_t hash(const uint64_t a) { return lintel::BobJenkinsHashMixULL(a); }

        template<class Head>
        inline uint32_t hash(const cons<Head, null_type> &v) {
            return hash(v.get_head());
        }

        // See http://burtleburtle.net/bob/c/lookup3.c for a discussion
        // about how we could have even fewer calls to the mix function.
        // Would want to upgrade to the newer hash function.
        template<class Head1, class Head2, class Tail>
        inline uint32_t hash(const cons<Head1, cons<Head2, Tail> > &v) {
            uint32_t a = hash(v.get_head());
            uint32_t b = hash(v.get_tail().get_head());
            uint32_t c = hash(v.get_tail().get_tail());
            return lintel::BobJenkinsHashMix3(a,b,c);
        }

        template<class BitSet>
        inline uint32_t partial_hash(const null_type &, const BitSet &, size_t) {
            return 0;
        }

        template<class Head, class BitSet>
        inline uint32_t partial_hash(const cons<Head, null_type> &v,
                                     const BitSet &used, size_t cur_pos) {
            return used[cur_pos] ? hash(v.get_head()) : 0;
        }

        template<class Head1, class Head2, class Tail, class BitSet>
        inline uint32_t partial_hash(const cons<Head1, cons<Head2, Tail> > &v,
                                     const BitSet &used, size_t cur_pos) {
            uint32_t a = used[cur_pos] ? hash(v.get_head()) : 0;
            uint32_t b = used[cur_pos+1] ? hash(v.get_tail().get_head()) : 0;
            uint32_t c = partial_hash(v.get_tail().get_tail(), used, cur_pos + 2);
            return lintel::BobJenkinsHashMix3(a,b,c);
        }

        template<class BitSet>
        inline bool partial_equal(const null_type &lhs, const null_type &rhs,
                                  const BitSet &used, size_t cur_pos) {
            return true;
        }

        template<class Head, class Tail, class BitSet>
        inline bool partial_equal(const cons<Head, Tail> &lhs,
                                  const cons<Head, Tail> &rhs,
                                  const BitSet &used, size_t cur_pos) {
            if (used[cur_pos] && lhs.get_head() != rhs.get_head()) {
                return false;
            }
            return partial_equal(lhs.get_tail(), rhs.get_tail(),
                                 used, cur_pos + 1);
        }

        template<class BitSet>
        inline bool partial_strict_less_than(const null_type &lhs,
                                             const null_type &rhs,
                                             const BitSet &used_lhs,
                                             const BitSet &used_rhs,
                                             size_t cur_pos) {
            return false;
        }

        template<class Head, class Tail, class BitSet>
        inline bool partial_strict_less_than(const cons<Head, Tail> &lhs,
                                             const cons<Head, Tail> &rhs,
                                             const BitSet &used_lhs,
                                             const BitSet &used_rhs,
                                             size_t cur_pos) {
            if (used_lhs[cur_pos] && used_rhs[cur_pos]) {
                if (lhs.get_head() < rhs.get_head()) {
                    return true;
                } else if (lhs.get_head() > rhs.get_head()) {
                    return false;
                } else {
                    // don't know, fall through to recursion.
                }
            } else if (used_lhs[cur_pos] && !used_rhs[cur_pos]) {
                return true; // used < *
            } else if (!used_lhs[cur_pos] && used_rhs[cur_pos]) {
                return false; // * > used
            }
            return partial_strict_less_than(lhs.get_tail(), rhs.get_tail(),
                                            used_lhs, used_rhs, cur_pos + 1);
        }
    } }

template<class Tuple> struct TupleHash {
    uint32_t operator()(const Tuple &a) const {
        return boost::tuples::hash(a);
    }
};

// Could make this a pair, but this makes it more obvious what's going on.
template<typename T>
struct AnyPair {
    bool any;
    T val;
    AnyPair() : any(true) { }
    AnyPair(const T &v) : any(false), val(v) { }
    void set(const T &v) { val = v; any = false; }

    bool operator ==(const AnyPair<T> &rhs) const {
        if (any != rhs.any) {
            return false;
        } else if (any) {
            return true;
        } else {
            return val == rhs.val;
        }
    }
    bool operator <(const AnyPair<T> &rhs) const {
        if (any && rhs.any) {
            return false;
        } else if (any && !rhs.any) {
            return false;
        } else if (!any && rhs.any) {
            return true;
        } else if (!any && !rhs.any) {
            return val < rhs.val;
        }
        FATAL_ERROR("?");
    }
};

template<class T>
inline uint32_t hash(const AnyPair<T> &v) {
    if (v.any) {
        return 0;
    } else {
        using boost::tuples::hash;
        return hash(v.val);
    }
}

template<class Tuple> struct PartialTuple {
    BOOST_STATIC_CONSTANT(uint32_t,
                          length = boost::tuples::length<Tuple>::value);

    Tuple data;
    typedef std::bitset<boost::tuples::length<Tuple>::value> UsedT;
    UsedT used;

    PartialTuple() { }
    // defaults to all unused
    explicit PartialTuple(const Tuple &from) : data(from) { }

    bool operator==(const PartialTuple &rhs) const {
        if (used != rhs.used) {
            return false;
        }
        return boost::tuples::partial_equal(data, rhs.data, used, 0);
    }
    bool operator<(const PartialTuple &rhs) const {
        return boost::tuples::partial_strict_less_than(data, rhs.data,
                                                       used, rhs.used, 0);
    }
};

template<class Tuple> struct PartialTupleHash {
    uint32_t operator()(const PartialTuple<Tuple> &v) const {
        return boost::tuples::partial_hash(v.data, v.used, 0);
    }
};

#endif


// 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.
//
// Algorithm inspired by Nick Vannieuwenhoven, written by Cornelius Steinhardt

#ifndef MTL_MATRIX_READ_EL_MATRIX
#define MTL_MATRIX_READ_EL_MATRIX

#include <string>

#include <iostream>
#include <istream>
#include <set>
#include <vector>
#include <valarray>

#include <boost/numeric/mtl/interface/vpt.hpp>
#include <boost/numeric/mtl/matrix/element.hpp>
#include <boost/numeric/mtl/matrix/element_structure.hpp>

namespace mtl { namespace mat {

// Read a value from the stream. The stream is advanced.
template <class T, class StreamType>
inline T read_value(StreamType& stream)
{
	T value;
	stream >> value;
	return value;
}

// Reads the element structure from a given file.
//
// It is assumed the nodes are numbered consecutively, i.e. there are no unused
// node numbers.
template < typename StreamType, typename ValueType>
void read_el_matrix(StreamType& file, element_structure<ValueType>& A)
{
    // Type definitions
    typedef element<ValueType>		element_type;
    // typedef typename element_type::value_type 	value_type;
    typedef typename element_type::index_type 	indices;
    typedef typename element_type::matrix_type 	matrix;
    vampir_trace<4036> trace;

    // Read element type information.
    int nb_elements = 0;
    file >> nb_elements;
    file.ignore(500,'\n');
    std::cout << "nb elements: " << nb_elements << "\n";

    // Compatibility with older files
    file.ignore(500,'\n');

    assert(nb_elements >= 0);

    element_type* elements = new element_type[nb_elements];

    // Read elements from file.
    int el_nbr = 0;
    int nb_total_vars = 0;
    while( el_nbr < nb_elements ) {

	// Read the node numbers.
	std::string line;
	getline(file, line, '\n');
	std::stringstream node_line(line), read_node_line(line);

	int read_num=0, i=0;
	while( !read_node_line.eof() ) {
	    int idx = 0;
	    read_node_line >> idx;
	    ++read_num;
	}
	read_num--;
	mtl::dense_vector<int> nodes(read_num, 0);
	while( !node_line.eof() ) {
	    int idx = 0;
	    node_line >> idx;
	    if (i<read_num)
		nodes[i]=idx;
	    if(idx > nb_total_vars)
		nb_total_vars = idx;
	    i++;
	}
	indices index(nodes);
	// Read the values.
	const int nb_vars = int(size(nodes));
	matrix vals(nb_vars, nb_vars);
	for(int i = 0; i < nb_vars*nb_vars; ++i)
	    vals(i / nb_vars, i % nb_vars) = read_value<ValueType>(file);
	file.ignore(500,'\n');
	file.ignore(500,'\n');
	element_type elem(el_nbr, index, vals);
	elements[el_nbr] = elem;
	if(el_nbr == 0){
	  std::cout<< "elem=" << elem << "\n";
	}
	++el_nbr;
    }

    // Construct mapping.
    ++nb_total_vars;
    assert(nb_total_vars >= 0);
    std::vector<int>* node_element_map = new std::vector<int>[nb_total_vars];
    for( int i = 0; i < nb_elements; ++i ) {
	element_type& el = elements[i];
	indices& idx = el.get_indices();
	for(std::size_t j = 0; j < el.nb_vars(); ++j)
	    node_element_map[ idx(j) ].push_back(el.get_id());
    }

    // Construct neighborhood information.
    for( int i = 0; i < nb_elements; ++i ) {
	element_type& el = elements[i];
	indices& idx = el.get_indices();
	std::set<int> neighs;
	for(std::size_t j = 0; j < el.nb_vars(); ++j)
	    neighs.insert(node_element_map[ idx(j) ].begin(),
			  node_element_map[ idx(j) ].end());

	for(std::set<int>::iterator it = neighs.begin(); it != neighs.end(); ++it)
	    if( *it != el.get_id() )
		el.get_neighbors().push_back( elements+(*it) );

	// Sort data.
	el.sort_indices();
    }

    delete[] node_element_map;
    A.consume(nb_elements, nb_total_vars, elements);
}

template <typename ValueType>
inline void read_el_matrix(std::string& mat_file, element_structure<ValueType>& A)
{    read_el_matrix(mat_file.c_str(), A);   }

template <typename ValueType>
void read_el_matrix(const char* mat_file, element_structure<ValueType>& A)
{
    std::ifstream file;
    file.open( mat_file );
    if( !file.is_open() ) {
	std::cout << "The file \"" << mat_file << "\" could not be opened." <<
	    std::endl;
	throw "File could not be opened";
    }
    read_el_matrix(file, A);

    file.close();
}

}} // end namespace mtl::matrix

#endif // MTL_MATRIX_READ_EL_MATRIX


// helper functionality for tests involving Eigen Arrays

// include guard
#ifndef EIGEN_TEST_HELPER_HPP
#define EIGEN_TEST_HELPER_HPP

// includes
#include <gtest/gtest.h>
#include <Eigen/Dense>

// allow ASSERT_NEAR with Eigen classes
namespace testing
{
namespace internal
{
template<typename Derived1, typename Derived2>
AssertionResult DoubleNearPredFormat(const char* expr1,
                                     const char* expr2,
                                     const char* abs_error_expr,
                                     const Eigen::DenseBase<Derived1>& val1,
                                     const Eigen::DenseBase<Derived2>& val2,
                                     double abs_error) {
  // evaluate expression templates
  auto val1_ = val1.eval();
  auto val2_ = val2.eval();

  // deal with special case of empty matrices (which are always equal)
  if ( (val1_.rows() * val1_.cols() == 0) &&
       (val2_.rows() * val2_.cols() == 0) &&
       (val1_.rows() == val2_.rows() ) &&
       (val1_.cols() == val2_.cols() ) )
    return AssertionSuccess();

  // handle different shapes
  if( val1_.rows() != val2_.rows() ||
      val1_.cols() != val2_.cols() )
    return AssertionFailure()
      << "Mismatching dimensions between " << expr1 << " and " << expr2 << ", where\n"
      << expr1 << " evaluates to\n" << val1 << ",\nand\n"
      << expr2 << " evaluates to\n" << val2 << ".";

  // let Eigen determine if they are equal enough
  auto diff = (val1_-val2_).array().abs().eval();
  if (diff.maxCoeff() <= abs_error) return AssertionSuccess();

  // TODO(wan): do not print the value of an expression if it's
  // already a literal.
  return AssertionFailure()
      << "The difference between " << expr1 << " and " << expr2
      << " is\n" << diff << ",\nwhich exceeds " << abs_error_expr << ", where\n"
      << expr1 << " evaluates to\n" << val1 << ",\n"
      << expr2 << " evaluates to\n" << val2 << ",\nand\n"
      << abs_error_expr << " evaluates to " << abs_error << ".";
}
}
}

#endif // EIGEN_TEST_HELPER_HPP


// Copyright (C) 2014 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <algorithm>
#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <string>
#include <unordered_map>
#include <utility>
#include <vector>

#include "theia/math/util.h"
#include "theia/sfm/camera/camera.h"
#include "theia/sfm/global_pose_estimation/LiGT_position_estimator.h"
#include "theia/sfm/reconstruction.h"
#include "theia/sfm/track.h"
#include "theia/sfm/transformation/align_point_clouds.h"
#include "theia/sfm/types.h"
#include "theia/util/map_util.h"
#include "theia/util/random.h"
#include "theia/util/stringprintf.h"
#include "theia/sfm/pose/test_util.h"
#include "gtest/gtest.h"

namespace theia {

using Eigen::Vector3d;

namespace {

RandomNumberGenerator rng(42);

Camera RandomCamera() {
  Camera camera;
  camera.SetPosition(10 * rng.RandVector3d());
  camera.SetOrientationFromAngleAxis(0.2 * rng.RandVector3d());
  camera.SetImageSize(1000, 1000);
  camera.SetFocalLength(800);
  camera.SetPrincipalPoint(500.0, 500.0);
  return camera;
}

Vector3d RelativeRotationFromTwoRotations(const Vector3d& rotation1,
                                          const Vector3d& rotation2,
                                          const double noise) {
  const Eigen::Matrix3d noisy_rotation =
      Eigen::AngleAxisd(DegToRad(noise), rng.RandVector3d().normalized())
          .toRotationMatrix();

  Eigen::Matrix3d rotation_matrix1, rotation_matrix2;
  ceres::AngleAxisToRotationMatrix(rotation1.data(), rotation_matrix1.data());
  ceres::AngleAxisToRotationMatrix(rotation2.data(), rotation_matrix2.data());

  const Eigen::AngleAxisd relative_rotation(noisy_rotation * rotation_matrix2 *
                                            rotation_matrix1.transpose());
  return relative_rotation.angle() * relative_rotation.axis();
}

Vector3d RelativeTranslationFromTwoPositions(const Vector3d& position1,
                                             const Vector3d& position2,
                                             const Vector3d& rotation1,
                                             const double noise) {
  const Eigen::AngleAxisd noisy_translation(DegToRad(noise),
                                            rng.RandVector3d().normalized());
  Eigen::Matrix3d rotation_matrix1;
  ceres::AngleAxisToRotationMatrix(rotation1.data(), rotation_matrix1.data());
  const Vector3d relative_translation =
      rotation_matrix1 * (position2 - position1).normalized();
  return noisy_translation * relative_translation;
}

// Aligns positions to the ground truth positions via a similarity
// transformation.
void AlignPositions(const std::unordered_map<ViewId, Vector3d>& gt_positions,
                    std::unordered_map<ViewId, Vector3d>* positions) {
  // Collect all positions into a vector.
  std::vector<Vector3d> gt_pos, pos;
  for (const auto& gt_position : gt_positions) {
    gt_pos.push_back(gt_position.second);
    const Vector3d& position = FindOrDie(*positions, gt_position.first);
    pos.push_back(position);
  }

  Eigen::Matrix3d rotation;
  Vector3d translation;
  double scale;
  AlignPointCloudsUmeyama(pos, gt_pos, &rotation, &translation, &scale);

  // Apply the similarity transformation.
  for (auto& position : *positions) {
    position.second = scale * (rotation * position.second) + translation;
  }
}

}  // namespace

class EstimatePositionsLiGTTest { // : public ::testing::Test {
 public:
   EstimatePositionsLiGTTest(const int num_views,
                           const int num_tracks,
                           const int num_view_pairs,
                           const double pose_noise,
                           const double image_noise,
                           const double position_tolerance) {
    // Set up the camera.
    SetupReconstruction(num_views, num_tracks, image_noise);
    GetTwoViewInfos(num_view_pairs, pose_noise);

    // Estimate the positions.
    LiGTPositionEstimator position_estimator(options_, reconstruction_);

    std::unordered_map<ViewId, Vector3d> estimated_positions;
    EXPECT_TRUE(position_estimator.EstimatePositions(
        view_pairs_, orientations_, &estimated_positions));
    EXPECT_EQ(estimated_positions.size(), positions_.size());

    // Align the positions and measure the error.
    AlignPositions(positions_, &estimated_positions);
    for (const auto& position : positions_) {
      const Vector3d& estimated_position =
          FindOrDie(estimated_positions, position.first);
      std::cout<< "\ng.t. position      = " << position.second.transpose()
      << "\nestimated position = " << estimated_position.transpose() <<"\n";
      const double position_error =
          (position.second - estimated_position).norm();
      EXPECT_LT(position_error, position_tolerance)
          << "\ng.t. position = " << position.second.transpose()
          << "\nestimated position = " << estimated_position.transpose();
    }
  }

 protected:
  void SetUp() {}

  void SetupReconstruction(const int num_views, const int num_tracks, const double image_noise) {
    // Create random views.
    std::vector<ViewId> view_ids;
    for (int i = 0; i < num_views; i++) {
      const ViewId view_id = reconstruction_.AddView(StringPrintf("%d", i), 0, i);
      view_ids.push_back(view_id);

      // Create a random pose.
      *reconstruction_.MutableView(view_id)->MutableCamera() = RandomCamera();
      orientations_[view_id] =
          reconstruction_.View(view_id)->Camera().GetOrientationAsAngleAxis();
      positions_[view_id] =
          reconstruction_.View(view_id)->Camera().GetPosition();
    }

    // Add random tracks.
    for (int i = 0; i < num_tracks; i++) {
      // Shuffle the view ids so that we can obtain tracks in random views.
      std::random_shuffle(view_ids.begin(), view_ids.end());

      // Create a track that is seen in several views.
      Eigen::Vector4d point = rng.RandVector4d();
      point[2] += 20.0;
      point[3] = 1.0;
      std::vector<std::pair<ViewId, Feature> > features;
      for (size_t j = 0; j < view_ids.size(); j++) {
        const View* view = reconstruction_.View(view_ids[j]);
        Eigen::Vector2d pixel;
        view->Camera().ProjectPoint(point, &pixel);
        AddNoiseToProjection(image_noise, &rng, &pixel);
        features.emplace_back(view_ids[j], pixel);
      }
      Track* track =
          reconstruction_.MutableTrack(reconstruction_.AddTrack(features));
      *track->MutablePoint() = point;
    }
  }

  void GetTwoViewInfos(const size_t num_view_pairs, const double pose_noise) {
    // Create a single connected component.
    std::vector<ViewId> view_ids;
    view_ids.push_back(0);
    for (size_t i = 1; i < positions_.size(); i++) {
      const ViewIdPair view_id_pair(i - 1, i);
      view_pairs_[view_id_pair] = CreateTwoViewInfo(view_id_pair, pose_noise);
      view_ids.push_back(i);
    }

    while (view_pairs_.size() < num_view_pairs) {
      std::random_shuffle(view_ids.begin(), view_ids.end());
      const ViewIdPair view_id_pair =
          (view_ids[0] < view_ids[1]) ? ViewIdPair(view_ids[0], view_ids[1])
                                      : ViewIdPair(view_ids[1], view_ids[0]);
      if (ContainsKey(view_pairs_, view_id_pair)) {
        continue;
      }

      view_pairs_[view_id_pair] = CreateTwoViewInfo(view_id_pair, pose_noise);
    }
  }

  TwoViewInfo CreateTwoViewInfo(const ViewIdPair& view_id_pair,
                                const double pose_noise) {
    CHECK_LT(view_id_pair.first, view_id_pair.second);
    TwoViewInfo info;
    info.focal_length_1 = 800.0;
    info.focal_length_2 = 800.0;

    // These objects will add noise to the relative pose.
    const Eigen::Vector2d noise = pose_noise * rng.RandVector2d();

    // Determine the relative rotation and add noise.
    info.rotation_2 = RelativeRotationFromTwoRotations(
        FindOrDie(orientations_, view_id_pair.first),
        FindOrDie(orientations_, view_id_pair.second),
        noise(0));

    // Determine the relative position and add noise.
    info.position_2 = RelativeTranslationFromTwoPositions(
        FindOrDie(positions_, view_id_pair.first),
        FindOrDie(positions_, view_id_pair.second),
        FindOrDie(orientations_, view_id_pair.first),
        noise(1));

    return info;
  }

  LiGTPositionEstimator::Options options_;
  std::unordered_map<ViewId, Vector3d> positions_;
  std::unordered_map<ViewId, Vector3d> orientations_;
  std::unordered_map<ViewIdPair, TwoViewInfo> view_pairs_;
  Reconstruction reconstruction_;
};

TEST(EstimatePositionsLiGTTest1, MinimalTestNoNoise) {
  static const double kTolerance = 1e-4;
  static const int kNumViews = 3;
  static const int kNumTracksPerView = 5;
  static const int kNumViewPairs = 3;
  EstimatePositionsLiGTTest test_class(kNumViews,
                              kNumTracksPerView,
                              kNumViewPairs,
                              0.0,
                              0.0,
                              kTolerance);
}

TEST(EstimatePositionsLiGTTest2, MinimalTestWithNoise) {
  static const double kTolerance = 0.25;
  static const int kNumViews = 3;
  static const int kNumTracksPerView = 5;
  static const int kNumViewPairs = 3;
  static const double kPoseNoiseDegrees = 1.00;
  static const double kImageNoiseDegrees = 0.5;
  EstimatePositionsLiGTTest test_class(kNumViews,
                              kNumTracksPerView,
                              kNumViewPairs,
                              kPoseNoiseDegrees,
                              kImageNoiseDegrees,
                              kTolerance);
}

TEST(EstimatePositionsLiGTTest3, TestNoNoise) {
  static const double kTolerance = 0.25;
  static const int kNumViews = 200;
  static const int kNumTracksPerView = 100;
  static const int kNumViewPairs = 500;
  EstimatePositionsLiGTTest test_class(kNumViews,
                              kNumTracksPerView,
                              kNumViewPairs,
                              0.0, 0.0,
                              kTolerance);
}

TEST(EstimatePositionsLiGTTest4, TestWithNoise) {
  static const double kTolerance = 0.5;
  static const int kNumViews = 200;
  static const int kNumTracksPerView = 100;
  static const int kNumViewPairs = 500;

  static const double kPoseNoiseDegrees = 1.00;
  static const double kImageNoiseDegrees = 1.0;
  EstimatePositionsLiGTTest test_class(kNumViews,
                              kNumTracksPerView,
                              kNumViewPairs,
                              kPoseNoiseDegrees,
                              kImageNoiseDegrees,
                              kTolerance);
}

}  // namespace theia


// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE

// Librerie standard
#include <array>
#include <cmath>
#include <functional>
#include <iostream>
#include <vector>

// Libreria Eigen Dense
#include <Eigen/Dense>

// Librerie Libfranka
#include <franka/duration.h>
#include <franka/exception.h>
#include <franka/model.h>
#include <franka/robot.h>

// Contiene definizioni di funzioni utili
#include "../examples/examples_common.h"

/**
 * Programma di base C++ per il controllo in coppia del robot Panda.
 *
 * Struttura del codice:
 * 1. Setup del robot
 * 2. Definizione dei parametri.
 * 3. Definizione del loop di controllo.
 * 4. Esecuzione del loop di controllo.
 *
 * Nota: il programma non realizza nessun tipo di controllo ma può essere utilizzato come punto di partenza
 * per lo sviluppo di un algoritmo di controllo. Per avere un riscontro pratico si consiglia di
 * leggere il programma "Cartesian_Impedance_Control" nella cartella CustomPrograms così come
 * tutti gli esempi della cartella examples.
 * Nota2: il Panda Robot compensa da se le coppie gravitazionali e gli attriti quindi non vanno considerati nel modello.
 *
 * Nota3: la documentazione della libreria Eigen3 è reperibile al link https://eigen.tuxfamily.org/dox/.
 * Se si è familiari con l'utilizzo del Matlab si consiglia di dare uno sguardo al seguente cheat sheet:
 * https://eigen.tuxfamily.org/dox/AsciiQuickReference.txt
 */

int main(int argc, char** argv) {

    if (argc != 2) {
        std::cerr << "Specificare l'indirizzo IP del robot." << std::endl;
        return -1;
    }

    try {
      // Documentazione classe franka::Robot https://frankaemika.github.io/libfranka/classfranka_1_1Robot.html
      // Documentazione classe franka::Model: https://frankaemika.github.io/libfranka/classfranka_1_1Model.html
      // Documentazione classe franka::RobotState: https://frankaemika.github.io/libfranka/structfranka_1_1RobotState.html

      // Connessione al robot
        franka::Robot robot(argv[1]);

      // Set collision behavior (https://frankaemika.github.io/libfranka/classfranka_1_1Robot.html#a168e1214ac36d74ac64f894332b84534)
        setDefaultBehavior(robot);

      // Modello cinematico e dinamico del robot
        franka::Model model = robot.loadModel();

      // Lettura dello stato attuale del robot
        franka::RobotState initial_state = robot.readOnce();

      // A partire da una variabile robot_state si possono ottenere diverse informazioni, tra cui:
      // initial_state.O_T_EE è la matrice di trasformazione omogenea che descrive la posa
      // dell'organo terminale rispetto alla terna zero del robot.

        Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));

      // Estrazione della posizione dalla matrice di trasformazione
        Eigen::Vector3d position_d(initial_transform.translation());

      // Estrazione dell'orientamento in forma di quaternione a partire dalla matrice di rotazione
        Eigen::Quaterniond orientation_d(initial_transform.linear());

      // Inizializzazione variabile time
        double time = 0.0;

      // Definizione della callback per il loop di controllo in coppia:
      // L'esempio definisce una lambda function ma nulla vieta di creare una funzione esterna.

        auto generic_callback = [&](const franka::RobotState& robot_state,franka::Duration duration) -> franka::Torques {

      /** Inizio callback: durante il loop di controllo la callback viene eseguita con una frequenza
       * di 1Khz. La variabile robot_state viene implicitamente aggiornata ogni loop con la funzione robot.readOnce() **/

      // Update time: https://frankaemika.github.io/libfranka/classfranka_1_1Duration.html
        time += duration.toSec();

        Eigen::VectorXd  tau_des(7);

        // Calcolo
        // coppie
        // di
        // controllo

        std::array<double, 7> tau_des_array{};
        Eigen::VectorXd::Map(&tau_des_array[0], 7) = tau_des;

        return tau_des_array;
        };

      // Avvio del loop di controllo:
      // Doc: https://frankaemika.github.io/libfranka/classfranka_1_1Robot.html#a5b5ba0a4f2bfd20be963b05622e629e1
        robot.control(generic_callback);

      } catch (const franka::Exception& ex) {
          // print exception
          std::cout << ex.what() << std::endl;
      }

      return 0;
}


// Copyright 2018 The Simons Foundation, Inc. - All Rights Reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//    http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NETKET_MOMENTUM_HPP
#define NETKET_MOMENTUM_HPP

#include <Eigen/Core>
#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <complex>
#include <iostream>
#include "abstract_optimizer.hpp"

namespace netket {

class Momentum : public AbstractOptimizer {
  int npar_;

  double eta_;
  double beta_;

  Eigen::VectorXd mt_;

 public:
  explicit Momentum(double eta = 0.001, double beta = 0.9)
      : eta_(eta), beta_(beta) {
    npar_ = -1;
    PrintParameters();
  }

  void PrintParameters() {
    InfoMessage() << "Momentum optimizer initialized with these parameters :"
                  << std::endl;
    InfoMessage() << "Learning Rate = " << eta_ << std::endl;
    InfoMessage() << "Beta = " << beta_ << std::endl;
  }

  void Init(int npar) override {
    npar_ = npar;
    mt_.setZero(npar_);
  }

  void Update(const Eigen::VectorXd &grad,
              Eigen::Ref<Eigen::VectorXd> pars) override {
    assert(npar_ > 0);

    mt_ = beta_ * mt_ + (1. - beta_) * grad;

    for (int i = 0; i < npar_; i++) {
      pars(i) -= eta_ * mt_(i);
    }
  }

  void Reset() override { mt_ = Eigen::VectorXd::Zero(npar_); }
};

}  // namespace netket

#endif


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 boost.simd.swar toolbox - enumerate SIMD Mode"

#include <boost/simd/toolbox/swar/include/functions/arith.hpp>
#include <boost/simd/sdk/simd/native.hpp>

#include <boost/type_traits/is_same.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/toolbox/constant/constant.hpp>
#include <boost/simd/include/functions/load.hpp>

NT2_TEST_CASE_TPL ( arith, BOOST_SIMD_SIMD_TYPES)
{
  using boost::simd::native;
  using boost::simd::arith;
  using boost::dispatch::meta::as_;
  using boost::simd::tag::arith_;
  using boost::simd::arith;

  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>               vT;
  typedef typename boost::dispatch::meta::call<arith_(T, as_<vT> )>::type r_t;

  for(std::size_t i=0; i < vT::static_size;++i)
    NT2_TEST_EQUAL(arith<vT>()[i], T(i));
  for(std::size_t i=0; i < vT::static_size;++i)
    NT2_TEST_EQUAL(arith<vT>(T(10))[i], T(i+10));
  for(std::size_t i=0; i < vT::static_size;++i)
    NT2_TEST_EQUAL(arith<vT>(T(10), T(2.0))[i], T(10 + T(2.0)*i));
}

NT2_TEST_CASE_TPL ( arith_from_int, BOOST_SIMD_SIMD_TYPES)
{
  using boost::simd::native;
  using boost::simd::arith;
  using boost::dispatch::meta::as_;
  using boost::simd::tag::arith_;

  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>               vT;
  typedef typename boost::dispatch::meta::call<arith_(int, as_<vT> )>::type r_t;

  for(std::size_t i=0; i < vT::static_size;++i)
    NT2_TEST_EQUAL(arith<vT>(int(10))[i], T(10 + i));
  for(std::size_t i=0; i < vT::static_size;++i)
    NT2_TEST_EQUAL(arith<vT>(int(10), T(2.0))[i], T(10 + T(2.0)*i));
}


#pragma once
#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include <Eigen/Sparse>

#include "../Univariate.hpp"
#include "../Variable.hpp"
#include "../utils.hpp"

#include "DiagonalOperator.hpp"
#include "Operator.hpp"

#include <memory>

namespace yavque
{

class DiagonalHamEvol final : public Operator, public Univariate
{
private:
	bool conjugate_ = false;
	std::shared_ptr<const detail::DiagonalOperatorImpl> ham_;

	void dagger_in_place_impl() override { conjugate_ = !conjugate_; }

	DiagonalHamEvol(const DiagonalHamEvol&) = default;
	DiagonalHamEvol(DiagonalHamEvol&&) = default;

public:
	explicit DiagonalHamEvol(const DiagonalOperator& ham)
		: Operator(ham.dim(), "DiagonalHamEvol of " + ham.name()), ham_{ham.get_impl()}
	{
	}

	explicit DiagonalHamEvol(const DiagonalOperator& ham, Variable var)
		: Operator(ham.dim(), "DiagonalHamEvol of " + ham.name()),
		  Univariate(std::move(var)), ham_{ham.get_impl()}
	{
	}

	DiagonalHamEvol& operator=(const DiagonalHamEvol&) = delete;
	DiagonalHamEvol& operator=(DiagonalHamEvol&&) = delete;

	~DiagonalHamEvol() override = default;

	[[nodiscard]] DiagonalOperator hamiltonian() const { return DiagonalOperator(ham_); }

	[[nodiscard]] std::unique_ptr<Operator> clone() const override
	{
		auto p = std::unique_ptr<DiagonalHamEvol>(new DiagonalHamEvol(*this));
		p->change_variable(Variable{var_.value()});
		return p;
	}

	[[nodiscard]] std::unique_ptr<Operator> log_deriv() const override
	{
		constexpr std::complex<double> I(0., 1.0);
		std::string op_name = std::string("derivative of ") + name(); // change to fmt
		cx_double constant = conjugate_ ? I : -I;
		return std::make_unique<DiagonalOperator>(ham_, op_name, constant);
	}

	[[nodiscard]] Eigen::VectorXcd apply_right(const Eigen::VectorXcd& st) const override
	{
		constexpr std::complex<double> I(0., 1.0);
		double t = conjugate_ ? -var_.value() : var_.value();
		return exp(-I * ham_->get_diag_op().array() * t) * st.array();
	}

	[[nodiscard]] bool can_merge(const Operator& rhs) const override
	{
		if(const auto* p = dynamic_cast<const DiagonalHamEvol*>(&rhs))
		{
			if(ham_ == p->ham_)
			{
				return true;
			}
		}
		return false;
	}

	[[nodiscard]] std::string desc() const override
	{
		std::ostringstream ss;
		ss << "[" << name() << ", variable name: " << var_.name() << ", "
		   << "variable value: " << var_.value() << "]";
		return ss.str();
	}
};
} // namespace yavque