#ifndef DEMO
#define DEMO

#include <Eigen/Dense>
#include <algorithm>
#include <iostream>

#include "MiniDnn/layer.h"
#include "MiniDnn/layer/conv.h"
#include "MiniDnn/layer/identity_block.h"
#include "MiniDnn/layer/conv_block.h"
#include "MiniDnn/layer/fully_connected.h"
#include "MiniDnn/layer/ave_pooling.h"
#include "MiniDnn/layer/max_pooling.h"
#include "MiniDnn/layer/relu.h"
#include "MiniDnn/layer/sigmoid.h"
#include "MiniDnn/layer/softmax.h"
#include "MiniDnn/loss.h"
#include "MiniDnn/loss/mse_loss.h"
#include "MiniDnn/loss/cross_entropy_loss.h"
#include "MiniDnn/mnist.h"
#include "MiniDnn/network.h"
#include "MiniDnn/optimizer.h"
#include "MiniDnn/optimizer/sgd.h"
#include "Enclave.h"
#include "Enclave_t.h"  /* print_string */
//#include <time.h>

using namespace std;

class RandData {
public:
    Matrix train_data;
    Matrix train_labels;
    Matrix test_data;
    Matrix test_labels;

    RandData(int size) {
        train_data = Matrix::Ones(224 * 224 * 3, size);
        train_labels = Matrix::Ones(1, size);
        test_data = Matrix::Ones(224 * 224 * 3, size);
        test_labels = Matrix::Ones(1, size);
    }
};

void ecall_ml_resnet50() {
    // data
//    MNIST dataset("/Users/rc/Study/Projects/mini-dnn/data/mnist/");
//    dataset.read();
//    int n_train = dataset.train_data.cols();
//    int dim_in = dataset.train_data.rows();
//    std::cout << "mnist train number: " << n_train << std::endl;
//    std::cout << "mnist test number: " << dataset.test_labels.cols() << std::endl;
    RandData dataset(100);
//    dataset.read();
    int n_train = dataset.train_data.cols();
    int dim_in = dataset.train_data.rows();
    // dnn
    Network dnn;

//    // Conv
//    // int channel_in, int height_in, int width_in, int channel_out, int height_kernel,
//    // int width_kernel, int stride = 1, int pad_w = 0, int pad_h = 0

    Layer *input_conv1 = new Conv(3, 224, 224, 64, 7, 7, 2, 3, 3);
    Layer *input_relu1 = new ReLU;

    Layer *b1_pool = new MaxPooling(64, 112, 112, 2, 2, 2);

    dnn.add_layer(input_conv1);
    dnn.add_layer(input_relu1);
    dnn.add_layer(b1_pool);

    //------------------------------------------------------------
    Layer *s1_conv_block = new ConvBlock(64, 56, 56, 64,
                                         64, 256, 3, 3, 1);
    Layer *s1_id_block1 = new IdentityBlock(256, 56, 56, 64, 64, 3, 3);
    Layer *s1_id_block2 = new IdentityBlock(256, 56, 56, 64, 64, 3, 3);

    dnn.add_layer(s1_conv_block);
    dnn.add_layer(s1_id_block1);
    dnn.add_layer(s1_id_block2);

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

    Layer *s2_conv_block = new ConvBlock(256, 56, 56, 128,
                                         128, 512, 3, 3, 2);
    Layer *s2_id_block1 = new IdentityBlock(512, 28, 28, 128, 128, 3, 3);
    Layer *s2_id_block2 = new IdentityBlock(512, 28, 28, 128, 128, 3, 3);

    dnn.add_layer(s2_conv_block);
    dnn.add_layer(s2_id_block1);
    dnn.add_layer(s2_id_block2);

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

    Layer *s3_conv_block = new ConvBlock(512, 28, 28, 256,
                                         256, 1024, 3, 3, 2);
    Layer *s3_id_block1 = new IdentityBlock(1024, 14, 14, 256, 256, 3, 3);
    Layer *s3_id_block2 = new IdentityBlock(1024, 14, 14, 256, 256, 3, 3);

    dnn.add_layer(s3_conv_block);
    dnn.add_layer(s3_id_block1);
    dnn.add_layer(s3_id_block2);
    //------------------------------------------------------------

    Layer *s4_conv_block = new ConvBlock(1024, 14, 14, 512,
                                         512, 2048, 3, 3, 2);
    Layer *s4_id_block1 = new IdentityBlock(2048, 7, 7, 512, 512, 3, 3);
    Layer *s4_id_block2 = new IdentityBlock(2048, 7, 7, 512, 512, 3, 3);

    dnn.add_layer(s4_conv_block);
    dnn.add_layer(s4_id_block1);
    dnn.add_layer(s4_id_block2);
    //------------------------------------------------------------

    Layer *out_pool = new MaxPooling(2048, 7, 7, 7, 7, 1);

//    Layer *b1_pool = new MaxPooling(64, 28, 28, 2, 2, 2);
    Layer *fc_fc1 = new FullyConnected(out_pool->output_dim(), 1000);
    Layer *softmax = new Softmax;

    dnn.add_layer(out_pool);
    dnn.add_layer(fc_fc1);
    dnn.add_layer(softmax);

    // loss
    Loss *loss = new CrossEntropy;
    dnn.add_loss(loss);
    // train & test
    SGD opt(0.001, 5e-4, 0.9, true);
    // SGD opt(0.001);
    // const int n_epoch = 5;
    const int total_rounds[4] = {1, 2, 3, 4};
    const int batch_size = 20;
    for (int idx = 0; idx < 4; idx++) {
        int n_epoch = total_rounds[idx];
        printf("total_rounds: %d\n", 5 * n_epoch);
        ocall_start_clock();
        for (int epoch = 0; epoch < n_epoch; epoch++) {
            shuffle_data(dataset.train_data, dataset.train_labels);
            for (int start_idx = 0; start_idx < n_train; start_idx += batch_size) {
            // for(int start_idx=0, rounds=0; rounds < total_rounds; start_idx += batch_size){
                int ith_batch = start_idx / batch_size;
                Matrix x_batch = dataset.train_data.block(0, start_idx, dim_in,
                                                          std::min(batch_size, n_train - start_idx));
                Matrix label_batch = dataset.train_labels.block(0, start_idx, 1,
                                                                std::min(batch_size, n_train - start_idx));
                Matrix target_batch = one_hot_encode(label_batch, 1000);
                // if (false && ith_batch % 10 == 1) {
                //     std::cout << ith_batch << "-th grad: " << std::endl;
                //     dnn.check_gradient(x_batch, target_batch, 10);
                // }

                dnn.forward(x_batch);
                // ocall_end_clock("Forward: %f   ");

                // ocall_start_clock();
                dnn.backward(x_batch, target_batch);

    //            // display
    //            if (ith_batch % 2 == 0) {
    //                //std::cout << ith_batch << "-th batch, loss: " << dnn.get_loss() << std::endl;
    //                printf("%d-th batch, loss: %f\n", ith_batch, dnn.get_loss());
    //            }
                // optimize
                dnn.update(opt);
                // rounds++;
            }
        }
        ocall_end_clock("total: %f\n");

// //        // test
//         dnn.forward(dataset.test_data);
//         float acc = compute_accuracy(dnn.output(), dataset.test_labels);
        // std::cout << std::endl;
        // std::cout << epoch + 1 << "-th epoch, test acc: " << acc << std::endl;
        // std::cout << std::endl;
    }
}

#endif


#include <iostream>
#include <armadillo>
#include <fstream>
#include <string>
#include <vector>
#include <math.h>
#include <iomanip>

using namespace std;
using namespace arma;

struct GMS
{
    int G,M,S;

    GMS(int,int,int);
    double gdec();
    double rad();
};

struct topoPoint
{
    double Hz,Ze,Di;
    vec3 XYZ;
    bool rad;

    topoPoint(double,double,double,bool);
    void cart();
    void translate(vec3 tvec);
    void Rotate(mat R);

};

GMS::GMS (int g,int m,int s)
{
    G=g;
    M=m;
    S=s;
}

double GMS::gdec ()
{
    return double(G)+ (double(M)/60) + (double(S)/3600);
}

double GMS::rad()
{
    double radd = gdec()  * (datum::pi/180);
    return radd;
}

struct SMMTleverARM
{
//to handle with the base from the file
    vector<vector<string>> data;
    vector<vector<GMS>>  hor,zen;
    vector<vector<double>> dists;

//the arbitrary-frame surveyed points
    vector<vector<topoPoint>> points;

//the planes of the process (coefficients a,b,c,d)
    vec4 Hplan,Vplan;
    vec4 CEin,CEout,CDin,CDout;
//aux1 is parallel to Hplan

//matrices containing points
    mat ptsH,ptsV; //at the sides of the IMU
    mat ptsCEin,ptsCEout,ptsCDin,ptsCDout; //points on the parallel circular patterns, on the lens of the cameras
    mat ptsOnCEin,ptsOnCEout,ptsOnCDin,ptsOnCDout; //adjusted points

//rotation matrices
    mat Rot1 = eye<mat>(3,3);

//the principal vectors, to define the cannonical system
    vec3 u,v,w; //u for x,v for y,w for z

//the axis direction of each camera system, in the same convention of the IMU
    vec3 xE,yE,zE,xD,yD,zD;

//the axis of each camera system, with the z axis pointing backwards
    vec3 Xe,Ye,Ze,Xd,Yd,Zd;

//the difference between the antena top and the center of phase
    vec3 PhC = {0,0,-0.0071};

//the lever-arm vectors
    vec3 antLA,LcamLA,RcamLA;

//temporary, and auxiliar vectors
    vec3 tempE,tempD,aux1e,aux1d,aux_E0,aux_D0;

//the boresight matrices
    mat Rimu_LC = eye(3,3),Rimu_RC = eye(3,3),Rimu_LC2 = eye(3,3),Rimu_RC2 = eye(3,3),Rimu_LC3 = eye(3,3),Rimu_RC3 = eye(3,3);

//center of the IMU, the mark on the H plane, the mark on the v plane
    vec3 imuC,Hm,Vm;

    double planesAngle;

    SMMTleverARM (string fileName);

    vector<vector<string>> readLAFile(string filename);

//the member functions
    void horiz();
    void zenit();
    void distanc();
    void avgOBS();
    void pointsPrint(string filename);
    void translateAll(vec3 tvec);
    void rotAll (mat R);
    void report();

};

vec3 onPlanePoint(vec4 plCoef,double x,double y);

//all the function propotypes:
double pdpiH(double,double);
double pdpiV(double,double);
void TptMat (mat *M,vec Tvec);
void RptMat (mat *M,mat R);

vec4 plane3points(vec3 u,vec3 v,vec3 w);

mat topoPointsMat (vector<topoPoint> points,uword first,uword last);

vec4 parallelPlane(vec4 plane1,vec3 pointOnPlane);

vec3 ProjPtOrtPlane(vec3 pt2Proj,vec3 plNorm,vec3 ptOnPlane);

vec4 leasqPlane2(vector<topoPoint> pointList,mat *ptOnPlane,string repNam,uword ijpt=0)
{
    vec4 res,Xo;
    mat data,P,Pinf,A,B,M,mvcXa,mvcLa,mvcV,mvcW;
    vec Lb,W,X,Xa,K,V,La;
    double vp;

    if (pointList.size() >= 3)
    {
        Xo = plane3points(pointList.at(0).XYZ,pointList.at(1).XYZ,pointList.at(2).XYZ);
    }

    Pinf = eye(3,3)*10000;

    if (pointList.size() < 3)
    {
        res = {0,0,1,0};
    }
    else if (pointList.size() == 3)
    {
        res = Xo;
    }
    else
    {
        ofstream out(repNam);

        //vetor Lb
        data = ones(pointList.size(),3);

        for (uword i0 = 0; i0 < pointList.size(); i0++)
        {
            data.row(i0) = pointList.at(i0).XYZ.t();
        }

        Lb = vectorise(data.t());

        //matriz dos pesos
        P = eye(Lb.n_elem,Lb.n_elem);
        if (ijpt != 0)
        {
            //em caso de que haja um ponto a ser injuncionado
//        P.submat(ijpt*3-2,ijpt*3-2,ijpt*3,ijpt*3) = Pinf;
            P.submat(ijpt*3-3,ijpt*3-3,ijpt*3-1,ijpt*3-1) = Pinf;
        }
        //matriz jacobiana A (parametros)

        A = ones<mat>(pointList.size(),4);

        for (uword i = 0; i < pointList.size(); i++)
        {
            A(i,0)=data(i,0);
            A(i,1)=data(i,1);
            A(i,2)=data(i,2);
        }

        //matriz jacobiana B (observacoes)

        B = zeros<mat>(pointList.size(),Lb.n_elem);

        uword j = 0;
        for (uword i2 = 0; i2 < pointList.size(); i2++)
        {
            B(i2,0+j)=Xo(0);
            B(i2,1+j)=Xo(1);
            B(i2,2+j)=Xo(2);

            j += 3;
        }

        W = zeros<vec>(pointList.size());

        for (uword i3 = 0; i3 < pointList.size(); i3++)
        {
            W(i3) = dot(A.row(i3),Xo);
        }

        M = B * P.i() * B.t();

        X = - inv(A.t()*M.i()*A)*(A.t()*M.i()*W);

        Xa = Xo + X;

        K = -M.i()*(A*X+W);

        V = P.i() * B.t() * K;

        La = Lb+V;

        *ptOnPlane = trans(reshape(La,3,pointList.size()));

        vp = as_scalar( V.t() * P * V / (Lb.n_elem - Xo.n_elem) );

        mvcXa = vp * inv(A.t()*M.i()*A);

        mvcLa = vp*(inv(P)+inv(P)*B.t()*inv(M)*A*inv(A.t()*inv(M)*A)*A.t()*inv(M)*B*inv(P)-inv(P)*B.t()*inv(M)*B*inv(P));

        mvcV  = vp*P.i()-mvcLa;

        mvcW  = vp*M;

        out.precision(30);

        res = Xa;

//        Xo.raw_print(out,"Xo");
//        X.raw_print(out,"X");
//        Xa.raw_print(out,"Xa:");
//        res.raw_print(out," b ");

//        out <<"Xo+X: "<<endl<<std::setprecision(30)<< Xo+X<<endl;
        //normalizing the plane equations
        res.rows(0,2) = normalise(Xa.rows(0,2));
        res(3) = - dot(normalise(Xa.rows(0,2)),La.rows(0,2));

//            res.print(out," a ");

        data.print(out,"dados de entrada: ");
        out<<endl;

        res.print(out,"coeficientes do plano estimado: ");
        out<<endl;

        V.print(out,"resíduos: ");
        out<<endl;

        La.print(out,"La:");
        out<<endl;

        out << "var. posteriori: " << vp << endl<<endl;

        mvcXa.print(out,"MVC dos parametros ajustados");
        out<<endl;

        mvcLa.print(out,"MVC das observações ajustadas");
        out<<endl;

    }
//cout<< res<<endl;
    return res;
}

void leasq2ParallelPlanes(vec4 *plane1,vec4 *plane2,mat points1,mat points2,mat *ptOnPlane1,mat *ptOnPlane2,string repNam,uword n=5,double e=.000001)
{
    vec5 Xo,X,Xa;
    mat data,P,A,B,M,mvcXa,mvcLa,mvcV,mvcW,ptsOnPlanes;
    vec Lb,W,K,V,La,temp5,temp6;
    double vp;
    vec4 temp1,temp2,temp3,temp4;
    temp1.ones();
    temp2.zeros();
    uword iter;

    ofstream out(repNam);

    Xo.rows(0,3) = plane3points(trans(points1.row(0)),trans(points1.row(1)),trans(points1.row(2)) );
    Xo(4) = - dot(Xo.rows(0,2),points2.row(0));

    //vetor Lb

    data = join_vert(points1,points2);

    Lb = vectorise(data.t());

    //matriz dos pesos
    P = eye(Lb.n_elem,Lb.n_elem);

    //matriz jacobiana A (parametros)

    A = zeros<mat>(data.n_rows,Xo.n_elem);
    B = zeros<mat>(data.n_rows,Lb.n_elem);
    W = zeros<vec>(data.n_rows);

    A.submat(0,0,data.n_rows-1,data.n_cols-1) = data;

    for (uword i = 0; i < data.n_rows; i++)
    {
        if (i < points1.n_rows)
        {
            A(i,3) = 1;
        }
        else
        {
            A(i,4) = 1;
        }
    }

    for(uword it = 0; it < n; it ++)
    {

        uword j = 0;
        for (uword i2 = 0; i2 < data.n_rows; i2++)
        {
            B(i2,0+j)=Xo(0);
            B(i2,1+j)=Xo(1);
            B(i2,2+j)=Xo(2);

            j += 3;

            temp1.rows(0,2) = trans( data.row(i2) );
            temp2.rows(0,2) = Xo.rows(0,2);

            if(i2 < points1.n_rows)
            {
                temp2(3)=Xo(3);
            }
            else
            {
                temp2(3)=Xo(4);
            }

            W(i2) = dot(temp1,temp2);
        }

        M = B * P.i() * B.t();

        X = - inv(A.t()*M.i()*A)*(A.t()*M.i()*W);

        Xa = Xo + X;

        Xo = Xa;

        iter = it;

        if (arma::max(arma::abs(X)) < e)
        {
            break;
        }

    }

    K = -M.i()*(A*X+W);

    V = P.i() * B.t() * K;

    La = Lb+V;

    ptsOnPlanes = trans(reshape(La,data.n_cols,data.n_rows));

    *ptOnPlane1 = ptsOnPlanes.rows(0,points1.n_rows-1);

    *ptOnPlane2 = ptsOnPlanes.rows(points1.n_rows,data.n_rows-1);

    temp3.rows(0,2) =   normalise(Xa.rows(0,2));
    temp3(3)= - dot(La.rows(0,2),normalise(Xa.rows(0,2)));

    temp4 = temp3;
    temp4(3)= - dot(La.rows(La.n_elem-3,La.n_elem-1),normalise(Xa.rows(0,2)));

    *plane1 = temp3;
    *plane2 = temp4;

    vp = as_scalar( V.t() * P * V / (Lb.n_elem - Xo.n_elem) );

    mvcXa = vp * inv(A.t()*M.i()*A);

    mvcLa = vp*(inv(P)+inv(P)*B.t()*inv(M)*A*inv(A.t()*inv(M)*A)*A.t()*inv(M)*B*inv(P)-inv(P)*B.t()*inv(M)*B*inv(P));

    mvcV  = vp*P.i()-mvcLa;

    mvcW  = vp*M;

    out.precision(20);

    points1.raw_print(out,"dados de entrada (primeiro plano): ");
    out<<endl;

    points2.raw_print(out,"dados de entrada (segundo plano): ");
    out<<endl;

    out << iter <<" iterações necessárias" <<endl<<endl;

    Xa.raw_print(out," vetor Xa (pré-normalização)");
    out<<endl;

    temp3.raw_print(out,"coeficientes do primeiro plano estimado: ");
    out<<endl;

    temp4.raw_print(out,"coeficientes do segundo plano estimado: ");
    out<<endl;

    V.raw_print(out,"resíduos: ");
    out<<endl;

    ptsOnPlanes.raw_print(out,"La:");
    out<<endl;

    out << "var. posteriori: " << vp << endl<<endl;

    mvcXa.raw_print(out,"MVC dos parametros ajustados");
    out<<endl;

    mvcLa.raw_print(out,"MVC das observações ajustadas");
    out<<endl;

}

struct Sphere
{
    vec3 center;
    double r;
    mat onSpherePts;

    Sphere(mat points,string repNam,uword n=20,double e=.000001);
};

Sphere::Sphere(mat data,string repNam,uword n,double e)
{
    vec4 Xo,X,Xa;
    mat P,A,B,M,mvcXa,mvcLa,mvcV,mvcW,ptsOnPlanes;
    vec Lb,W,K,V,La,temp5,temp6,aux;
    double vp;
    vec4 temp1,temp2,temp3,temp4;
    temp1.ones();
    temp2.zeros();
    uword iter;

    ofstream out(repNam);

    aux=zeros(data.n_rows);

    for (uword i = 0; i < aux.n_rows; i++)
    {
        aux(i) = norm(data.row(i) - mean(data));
    }

    Xo.rows(1,3) = trans(mean(data));
    Xo(0) = mean(aux);

    //vetor Lb

    Lb = vectorise(data.t());

    //matriz dos pesos
    P = eye(Lb.n_elem,Lb.n_elem);

    //matriz jacobiana A (parametros)

    A = zeros<mat>(data.n_rows,Xo.n_elem);
    B = zeros<mat>(data.n_rows,Lb.n_elem);
    W = zeros<vec>(data.n_rows);

    for(uword it = 0; it < n; it ++)
    {

        uword j = 0;
        for (uword i2 = 0; i2 < data.n_rows; i2++)
        {
            A(i2,0) = - 2 * Xo(0);

            A(i2,1) = 2 * (Xo(1) - data(i2,0));
            A(i2,2) = 2 * (Xo(2) - data(i2,1));
            A(i2,3) = 2 * (Xo(3) - data(i2,2));

            B(i2,j)  = - 2 * (Xo(1) - data(i2,0));
            B(i2,1+j)= - 2 * (Xo(2) - data(i2,1));
            B(i2,2+j)= - 2 * (Xo(3) - data(i2,2));

            j += 3;

            W(i2) = std::pow((data(i2,0) - Xo(1)),2) + std::pow((data(i2,1) - Xo(2)),2) +
                    std::pow((data(i2,2) - Xo(3)),2) - std::pow(Xo(0),2);
        }

        M = B * P.i() * B.t();

        X = - inv(A.t()*M.i()*A)*(A.t()*M.i()*W);

        Xa = Xo + X;

        Xo = Xa;

        iter = it;

        if (arma::max(arma::abs(X)) < e)
        {
            break;
        }

    }

    K = -M.i()*(A*X+W);

    V = P.i() * B.t() * K;

    La = Lb+V;

    onSpherePts = trans(reshape(La,data.n_cols,data.n_rows));

    vp = as_scalar( V.t() * P * V / (Lb.n_elem - Xo.n_elem) );

    mvcXa = vp * inv(A.t()*M.i()*A);

    mvcLa = vp*(inv(P)+inv(P)*B.t()*inv(M)*A*inv(A.t()*inv(M)*A)*A.t()*inv(M)*B*inv(P)-inv(P)*B.t()*inv(M)*B*inv(P));

    mvcV  = vp*P.i()-mvcLa;

    mvcW  = vp*M;

    out.precision(20);

    data.raw_print(out,"dados de entrada :");
    out<<endl;

    out << iter <<" iterações necessárias" <<endl<<endl;

    Xa.raw_print(out,"parametros ajustados (r,XC,YC,ZC)");
    out<<endl;

    V.raw_print(out,"resíduos: ");
    out<<endl;

    La.raw_print(out,"La:");
    out<<endl;

    out << "var. posteriori: " << vp << endl<<endl;

    mvcXa.raw_print(out,"MVC dos parametros ajustados");
    out<<endl;

    mvcLa.raw_print(out,"MVC das observações ajustadas");
    out<<endl;

    //assignment to the object variables
    r = Xo(0);
    center = Xo.rows(1,3);
}

vector<vector<string>> SMMTleverARM::readLAFile(string filename)
{
//to read a file of the raw data of the Lever Arm surveying
    ifstream in(filename);
    vector<vector<string>> res;
    vector<string> A,B,C,D,E;
//to control
    int state = 0;
    string line;

    while (std::getline(in,line))
    {
        if (line.find("COD:[A") != -1)
        {
            state = 1;
            continue;
        }
        if (line.find("COD:[B") != -1)
        {
            state = 2;
            continue;
        }
        if (line.find("COD:[C") != -1)
        {
            state =3 ;
            continue;
        }
        if (line.find("COD:[D") != -1)
        {
            state =4 ;
            continue;
        }
        if (line.find("COD:[E") != -1)
        {
            state =5 ;
            continue;
        }

        if (state == 1)
        {
            A.push_back(line);
            //cout << stoi(line.substr(23,3))<<endl;
        }
        if (state == 2)
        {
            B.push_back(line);
        }
        if (state == 3)
        {
            C.push_back(line);
        }
        if (state ==4 )
        {
            D.push_back(line);
        }
        if (state == 5)
        {
            E.push_back(line);
        }
    }

    res.push_back(A);
    res.push_back(B);
    res.push_back(C);
    res.push_back(D);
    res.push_back(E);
    //cout<<A.size()<<" "<< B.size()<<" "<<C.size()<<" "<<D.size()<<" "<<E.size()<<endl;
    //cout<<res.size();

    return res;
}

SMMTleverARM::SMMTleverARM (string fileName)
{
// the constructor, all the modifying functions must have their calls here.
    data = readLAFile(fileName);
    horiz();
    zenit();
    distanc();
    avgOBS();

//the two planes
    Vplan = leasqPlane2(points.at(1),&ptsV,"plano_vert.txt",2);
    Hplan = leasqPlane2(points.at(2),&ptsH,"plano_hor.txt",2);
//the angle between them
    planesAngle = std::acos(dot(Vplan.rows(0,2),Hplan.rows(0,2))) * (180/datum::pi);

//the marks on the IMU surface
    Hm = trans(ptsH.row(2));
    Vm = trans(ptsV.row(2));

//the normal of the Hplan, is also the third direction (w),
//but at first, is needed some test
    w = Hplan.rows(0,2);
    if(w(2)<0) //the z component, needs to be positive, due to the z axis of the total station
    {
        w *= -1;
    }

//the aux1 plane
//aux1.rows(0,2) = w;
//aux1(3) = - dot(w,Vm);

//IMU center with the intersection of the plane (aux1) and the line (point Hm and direction w)
//imuC = Hm + (dot(w,(Vm-Hm)))* w; //do not delete the old implementation
    imuC = ProjPtOrtPlane(Hm,w,Vm);

//the first principal direction (u)
    u = normalise(Vm - imuC);

//the second principal direction(v)
    v = normalise(cross(w,u));

//first, translating the origin to the calculated center of the IMU
    TptMat(&ptsH,-imuC);
    TptMat(&ptsV,-imuC);
    translateAll(-imuC);
    pointsPrint("points1.txt");

//composing the first rotation matrice
    Rot1.col(0)=u;
    Rot1.col(1)=v;
    Rot1.col(2)=w;
    Rot1 = Rot1.t();

//rotating all to the imu BF
    RptMat(&ptsH,Rot1);
    RptMat(&ptsV,Rot1);
    rotAll(Rot1);
    pointsPrint("points2.txt");

//imuC += -imuC;

//creating matrices with the points, from topoPoints;
    ptsCDin  = topoPointsMat(points.at(3),0,4);
    ptsCDout = topoPointsMat(points.at(3),5,9);
    ptsCEin  = topoPointsMat(points.at(4),0,4);
    ptsCEout = topoPointsMat(points.at(4),5,9);

//the pair of parallel planes, one for each camera
    leasq2ParallelPlanes(&CDin,&CDout,ptsCDin,ptsCDout,&ptsOnCDin,&ptsOnCDout,"RcamPlanes.txt");
    leasq2ParallelPlanes(&CEin,&CEout,ptsCEin,ptsCEout,&ptsOnCEin,&ptsOnCEout,"LcamPlanes.txt");

//creating a sphere for each camera, to measure the center
    Sphere Rsphere(join_vert(ptsOnCDin,ptsOnCDout),"Rsphere.txt");
    Sphere Lsphere(join_vert(ptsOnCEin,ptsOnCEout),"Lsphere.txt");

//Y of each camera direction (with same convention)
//due to the IMU bf and the cameras position, the y component needs to be positive
// to point forward the camera axis

    yE = normalise(CEin.rows(0,2));
    if (yE(1) < 0 )
    {
        yE *= -1;
    }

    yD = normalise(CDin.rows(0,2));
    if (yD(1) < 0 )
    {
        yD *= -1;
    }

//yD.print();cout<<endl;
//yE.print();cout<<endl;

//we'll need points perfectly on the plane, and to have this guarantee:
//each one are obtained by the plane equation, with z=f(x,y)
    aux1d = onPlanePoint(CDin,ptsOnCDin(ptsOnCDin.n_rows-1,0),ptsOnCDin(ptsOnCDin.n_rows-1,1));
    aux1e = onPlanePoint(CEin,ptsOnCEin(ptsOnCEin.n_rows-1,0),ptsOnCEin(ptsOnCEin.n_rows-1,1));

    aux_D0 = onPlanePoint(CDin,ptsOnCDin(0,0),ptsOnCDin(0,1));
    aux_E0 = onPlanePoint(CEin,ptsOnCEin(0,0),ptsOnCEin(0,1));

//projecting the center of spheres on the inner plane of the rings on the camera lenses
    tempD = ProjPtOrtPlane(Rsphere.center,yD,aux1d);
    tempE = ProjPtOrtPlane(Lsphere.center,yE,aux1e);

//calculating the z direction, for each camera
    zE = normalise(aux_E0 - tempE);
    zD = normalise(aux_D0 - tempD);

//and, finally the x direction
    xE = normalise(cross(yE,zE));
    xD = normalise(cross(yD,zD));

    vec3 Xe2,Ye2,Ze2,Xd2,Yd2,Zd2;

//////formerly:
//////we can describe the axes in the camera conventional convention (z axis pointing from the CP to the focal plane)
////Xe = xE;Ye = zE;Ze = -yE;
////Xd = xD;Yd = zD;Zd = -yD;

//we can describe the axes in the camera modern convention (z axis pointing from the principle point to CP)
    Xe = xE;
    Ye = -zE;
    Ze = yE;

    Xd = xD;
    Yd = -zD;
    Zd = yD;

    //conventional convention
    Xe2 = xE;Ye2 = zE;Ze2 = -yE;
    Xd2 = xD;Yd2 = zD;Zd2 = -yD;

//with the axis, we can have the boresight matrices:
    //first with the same convention of the IMU:
    Rimu_LC2.row(0) = trans(xE);
    Rimu_LC2.row(1) = trans(yE);
    Rimu_LC2.row(2) = trans(zE);

    Rimu_RC2.row(0) = trans(xD);
    Rimu_RC2.row(1) = trans(yD);
    Rimu_RC2.row(2) = trans(zD);

    //with the camera modern convention
    Rimu_LC.row(0) = trans(Xe);
    Rimu_LC.row(1) = trans(Ye);
    Rimu_LC.row(2) = trans(Ze);

    Rimu_RC.row(0) = trans(Xd);
    Rimu_RC.row(1) = trans(Yd);
    Rimu_RC.row(2) = trans(Zd);

    //then with the camera conventional convention
    Rimu_LC3.row(0) = trans(Xe2);
    Rimu_LC3.row(1) = trans(Ye2);
    Rimu_LC3.row(2) = trans(Ze2);

    Rimu_RC3.row(0) = trans(Xd2);
    Rimu_RC3.row(1) = trans(Yd2);
    Rimu_RC3.row(2) = trans(Zd2);

//now, the lever-arm

    //antenna lever-arm: just the point at the antena top, and the offset from there to the PhC
    antLA = (points.at(0).at(0).XYZ) + PhC;

    //the lever-arm of the two cameras:
    RcamLA = Rsphere.center;
    LcamLA = Lsphere.center;

    report();
}

void SMMTleverARM::avgOBS()
{
    vector<topoPoint> temp;

    double hA,zA,dA=0,hB,zB,dB,hC,zC,dC,hD,zD,dD,hE,zE,dE,hF,zF,dF,hG,zG,dG,hH,zH,dH,hI,zI,dI,hJ,zJ,dJ;

//std::setprecision(8);
    for (unsigned int i = 0; i < hor.size(); i++)
    {
        if (i == 0)
        {
            hA = (pdpiH(hor[i][0].gdec(),hor[i][1].gdec()) +
                  pdpiH(hor[i][2].gdec(),hor[i][3].gdec()) +
                  pdpiH(hor[i][4].gdec(),hor[i][5].gdec())) / 3;

            zA = (pdpiV(zen[i][0].gdec(),zen[i][1].gdec()) +
                  pdpiV(zen[i][2].gdec(),zen[i][3].gdec()) +
                  pdpiV(zen[i][4].gdec(),zen[i][5].gdec())) / 3;

            for (unsigned int j = 0; j < dists[i].size(); j++)
            {
                dA += dists[i][j]/6;
            }

            topoPoint ANT(hA,zA,dA,false);
            temp.push_back(ANT);

            //cout <<hA<<" "<<zA<<" "<<dA<<endl;
            //cout <<hA<<" "<<zA<<" "<<dA<<endl;
        }
        if (i == 1)
        {
            hA = (pdpiH(hor[i][0].gdec(),hor[i][5].gdec()) +
                  pdpiH(hor[i][10].gdec(),hor[i][15].gdec())) / 2;

            hB = (pdpiH(hor[i][1].gdec(),hor[i][6].gdec()) +
                  pdpiH(hor[i][11].gdec(),hor[i][16].gdec())) / 2;

            hC = (pdpiH(hor[i][2].gdec(),hor[i][7].gdec()) +
                  pdpiH(hor[i][12].gdec(),hor[i][17].gdec())) / 2;

            hD = (pdpiH(hor[i][3].gdec(),hor[i][8].gdec()) +
                  pdpiH(hor[i][13].gdec(),hor[i][18].gdec())) / 2;

            hE = (pdpiH(hor[i][4].gdec(),hor[i][9].gdec()) +
                  pdpiH(hor[i][14].gdec(),hor[i][19].gdec())) / 2;

            zA = (pdpiV(zen[i][0].gdec(),zen[i][5].gdec()) +
                  pdpiV(zen[i][10].gdec(),zen[i][15].gdec())) / 2;

            zB = (pdpiV(zen[i][1].gdec(),zen[i][6].gdec()) +
                  pdpiV(zen[i][11].gdec(),zen[i][16].gdec())) / 2;

            zC = (pdpiV(zen[i][2].gdec(),zen[i][7].gdec()) +
                  pdpiV(zen[i][12].gdec(),zen[i][17].gdec())) / 2;

            zD = (pdpiV(zen[i][3].gdec(),zen[i][8].gdec()) +
                  pdpiV(zen[i][13].gdec(),zen[i][18].gdec())) / 2;

            zE = (pdpiV(zen[i][4].gdec(),zen[i][9].gdec()) +
                  pdpiV(zen[i][14].gdec(),zen[i][19].gdec())) / 2;

            dA = (dists[i][0]+dists[i][5]+dists[i][10]+dists[i][15]) / 4;

            dB = (dists[i][1]+dists[i][6]+dists[i][11]+dists[i][16]) / 4;

            dC = (dists[i][2]+dists[i][7]+dists[i][12]+dists[i][17]) / 4;

            dD = (dists[i][3]+dists[i][8]+dists[i][13]+dists[i][18]) / 4;

            dE = (dists[i][4]+dists[i][9]+dists[i][14]+dists[i][19]) / 4;

            topoPoint A(hA,zA,dA,false);
            temp.push_back(A);
            topoPoint B(hB,zB,dB,false);
            temp.push_back(B);
            topoPoint C(hC,zC,dC,false);
            temp.push_back(C);
            topoPoint D(hD,zD,dD,false);
            temp.push_back(D);
            topoPoint E(hE,zE,dE,false);
            temp.push_back(E);

//                                cout <<hA<<" "<<zA<<" "<<dA<<endl;

        }
        if (i == 2)
        {
            hA = (pdpiH(hor[i][0].gdec(),hor[i][5].gdec()) +
                  pdpiH(hor[i][10].gdec(),hor[i][15].gdec())) / 2;

            hB = (pdpiH(hor[i][1].gdec(),hor[i][6].gdec()) +
                  pdpiH(hor[i][11].gdec(),hor[i][16].gdec())) / 2;

            hC = (pdpiH(hor[i][2].gdec(),hor[i][7].gdec()) +
                  pdpiH(hor[i][12].gdec(),hor[i][17].gdec())) / 2;

            hD = (pdpiH(hor[i][3].gdec(),hor[i][8].gdec()) +
                  pdpiH(hor[i][13].gdec(),hor[i][18].gdec())) / 2;

            hE = (pdpiH(hor[i][4].gdec(),hor[i][9].gdec()) +
                  pdpiH(hor[i][14].gdec(),hor[i][19].gdec())) / 2;

            zA = (pdpiV(zen[i][0].gdec(),zen[i][5].gdec()) +
                  pdpiV(zen[i][10].gdec(),zen[i][15].gdec())) / 2;

            zB = (pdpiV(zen[i][1].gdec(),zen[i][6].gdec()) +
                  pdpiV(zen[i][11].gdec(),zen[i][16].gdec())) / 2;

            zC = (pdpiV(zen[i][2].gdec(),zen[i][7].gdec()) +
                  pdpiV(zen[i][12].gdec(),zen[i][17].gdec())) / 2;

            zD = (pdpiV(zen[i][3].gdec(),zen[i][8].gdec()) +
                  pdpiV(zen[i][13].gdec(),zen[i][18].gdec())) / 2;

            zE = (pdpiV(zen[i][4].gdec(),zen[i][9].gdec()) +
                  pdpiV(zen[i][14].gdec(),zen[i][19].gdec())) / 2;

            dA = (dists[i][0]+dists[i][5]+dists[i][10]+dists[i][15]) / 4;

            dB = (dists[i][1]+dists[i][6]+dists[i][11]+dists[i][16]) / 4;

            dC = (dists[i][2]+dists[i][7]+dists[i][12]+dists[i][17]) / 4;

            dD = (dists[i][3]+dists[i][8]+dists[i][13]+dists[i][18]) / 4;

            dE = (dists[i][4]+dists[i][9]+dists[i][14]+dists[i][19]) / 4;

            topoPoint A(hA,zA,dA,false);
            temp.push_back(A);
            topoPoint B(hB,zB,dB,false);
            temp.push_back(B);
            topoPoint C(hC,zC,dC,false);
            temp.push_back(C);
            topoPoint D(hD,zD,dD,false);
            temp.push_back(D);
            topoPoint E(hE,zE,dE,false);
            temp.push_back(E);

//                                cout <<hA<<" "<<zA<<" "<<dA<<endl;

        }
        if (i == 3)
        {
            topoPoint EXTRA(hor[i][0].gdec(),zen[i][0].gdec(),dists[i][0],false);

            hA = pdpiH(hor[i][1].gdec(),hor[i][11].gdec());
            hB = pdpiH(hor[i][2].gdec(),hor[i][12].gdec());
            hC = pdpiH(hor[i][3].gdec(),hor[i][13].gdec());
            hD = pdpiH(hor[i][4].gdec(),hor[i][14].gdec());
            hE = pdpiH(hor[i][5].gdec(),hor[i][15].gdec());
            hF = pdpiH(hor[i][6].gdec(),hor[i][16].gdec());
            hG = pdpiH(hor[i][7].gdec(),hor[i][17].gdec());
            hH = pdpiH(hor[i][8].gdec(),hor[i][18].gdec());
            hI = pdpiH(hor[i][9].gdec(),hor[i][19].gdec());
            hJ = pdpiH(hor[i][10].gdec(),hor[i][20].gdec());

            zA = pdpiV(zen[i][1].gdec(),zen[i][11].gdec());
            zB = pdpiV(zen[i][2].gdec(),zen[i][12].gdec());
            zC = pdpiV(zen[i][3].gdec(),zen[i][13].gdec());
            zD = pdpiV(zen[i][4].gdec(),zen[i][14].gdec());
            zE = pdpiV(zen[i][5].gdec(),zen[i][15].gdec());
            zF = pdpiV(zen[i][6].gdec(),zen[i][16].gdec());
            zG = pdpiV(zen[i][7].gdec(),zen[i][17].gdec());
            zH = pdpiV(zen[i][8].gdec(),zen[i][18].gdec());
            zI = pdpiV(zen[i][9].gdec(),zen[i][19].gdec());
            zJ = pdpiV(zen[i][10].gdec(),zen[i][20].gdec());

            dA = (dists[i][1]+dists[i][11]) / 2;
            dB = (dists[i][2]+dists[i][12]) / 2;
            dC = (dists[i][3]+dists[i][13]) / 2;
            dD = (dists[i][4]+dists[i][14]) / 2;
            dE = (dists[i][5]+dists[i][15]) / 2;
            dF = (dists[i][6]+dists[i][16]) / 2;
            dG = (dists[i][7]+dists[i][17]) / 2;
            dH = (dists[i][8]+dists[i][18]) / 2;
            dI = (dists[i][9]+dists[i][19]) / 2;
            dJ = (dists[i][10]+dists[i][20]) / 2;

            topoPoint A(hA,zA,dA,false);
            temp.push_back(A);
            topoPoint B(hB,zB,dB,false);
            temp.push_back(B);
            topoPoint C(hC,zC,dC,false);
            temp.push_back(C);
            topoPoint D(hD,zD,dD,false);
            temp.push_back(D);
            topoPoint E(hE,zE,dE,false);
            temp.push_back(E);
            topoPoint F(hF,zF,dF,false);
            temp.push_back(F);
            topoPoint G(hG,zG,dG,false);
            temp.push_back(G);
            topoPoint H(hH,zH,dH,false);
            temp.push_back(H);
            topoPoint I(hI,zI,dI,false);
            temp.push_back(I);
            topoPoint J(hJ,zJ,dJ,false);
            temp.push_back(J);

            temp.push_back(EXTRA);

//                               cout <<hA<<" "<<zA<<" "<<dA<<endl;

        }
        if (i == 4)
        {
            topoPoint EXTRA(hor[i][0].gdec(),zen[i][0].gdec(),dists[i][0],false);

            hA = pdpiH(hor[i][1].gdec(),hor[i][11].gdec());
            hB = pdpiH(hor[i][2].gdec(),hor[i][12].gdec());
            hC = pdpiH(hor[i][3].gdec(),hor[i][13].gdec());
            hD = pdpiH(hor[i][4].gdec(),hor[i][14].gdec());
            hE = pdpiH(hor[i][5].gdec(),hor[i][15].gdec());
            hF = pdpiH(hor[i][6].gdec(),hor[i][16].gdec());
            hG = pdpiH(hor[i][7].gdec(),hor[i][17].gdec());
            hH = pdpiH(hor[i][8].gdec(),hor[i][18].gdec());
            hI = pdpiH(hor[i][9].gdec(),hor[i][19].gdec());
            hJ = pdpiH(hor[i][10].gdec(),hor[i][20].gdec());

            zA = pdpiV(zen[i][1].gdec(),zen[i][11].gdec());
            zB = pdpiV(zen[i][2].gdec(),zen[i][12].gdec());
            zC = pdpiV(zen[i][3].gdec(),zen[i][13].gdec());
            zD = pdpiV(zen[i][4].gdec(),zen[i][14].gdec());
            zE = pdpiV(zen[i][5].gdec(),zen[i][15].gdec());
            zF = pdpiV(zen[i][6].gdec(),zen[i][16].gdec());
            zG = pdpiV(zen[i][7].gdec(),zen[i][17].gdec());
            zH = pdpiV(zen[i][8].gdec(),zen[i][18].gdec());
            zI = pdpiV(zen[i][9].gdec(),zen[i][19].gdec());
            zJ = pdpiV(zen[i][10].gdec(),zen[i][20].gdec());

            dA = (dists[i][1]+dists[i][11]) / 2;
            dB = (dists[i][2]+dists[i][12]) / 2;
            dC = (dists[i][3]+dists[i][13]) / 2;
            dD = (dists[i][4]+dists[i][14]) / 2;
            dE = (dists[i][5]+dists[i][15]) / 2;
            dF = (dists[i][6]+dists[i][16]) / 2;
            dG = (dists[i][7]+dists[i][17]) / 2;
            dH = (dists[i][8]+dists[i][18]) / 2;
            dI = (dists[i][9]+dists[i][19]) / 2;
            dJ = (dists[i][10]+dists[i][20]) / 2;

            topoPoint A(hA,zA,dA,false);
            temp.push_back(A);
            topoPoint B(hB,zB,dB,false);
            temp.push_back(B);
            topoPoint C(hC,zC,dC,false);
            temp.push_back(C);
            topoPoint D(hD,zD,dD,false);
            temp.push_back(D);
            topoPoint E(hE,zE,dE,false);
            temp.push_back(E);
            topoPoint F(hF,zF,dF,false);
            temp.push_back(F);
            topoPoint G(hG,zG,dG,false);
            temp.push_back(G);
            topoPoint H(hH,zH,dH,false);
            temp.push_back(H);
            topoPoint I(hI,zI,dI,false);
            temp.push_back(I);
            topoPoint J(hJ,zJ,dJ,false);
            temp.push_back(J);

            temp.push_back(EXTRA);

//                               cout <<hA<<" "<<zA<<" "<<dA<<endl;

        }

        points.push_back(temp);
        temp.clear();
    }

    pointsPrint("points0.txt");
}

void SMMTleverARM::pointsPrint(string filename)
{
    ofstream outFile(filename);

    for (unsigned ii = 0; ii < points.size(); ii++)
    {
        for (unsigned jj = 0; jj < points[ii].size(); jj++)
        {
            outFile << points[ii][jj].XYZ(0) <<" ";
            outFile << points[ii][jj].XYZ(1) <<" ";
            outFile << points[ii][jj].XYZ(2) << endl;
        }
    }

    outFile.close();
}

void SMMTleverARM::horiz()
{
    vector<GMS> temp;
    int g,m,s;

    for (unsigned int i = 0; i < data.size() ; i++)
    {

        for (unsigned int j = 0; j < data[i].size(); j++)
        {

            g = stoi(data[i][j].substr(23,3));
            m = stoi(data[i][j].substr(27,2));
            s = stoi(data[i][j].substr(29,2));
            GMS temp2(g,m,s);

            temp.push_back(temp2);
        }
        hor.push_back(temp);
        temp.clear();
    }

}

void SMMTleverARM::zenit()
{
    vector<GMS> temp;
    int g,m,s;

    for (unsigned int i = 0; i < data.size() ; i++)
    {

        for (unsigned int j = 0; j < data[i].size(); j++)
        {

            g = stoi(data[i][j].substr(35,3));
            m = stoi(data[i][j].substr(39,2));
            s = stoi(data[i][j].substr(41,2));
            GMS temp2(g,m,s);
//            cout<<temp2.gdec()<<endl;

            temp.push_back(temp2);
        }
        zen.push_back(temp);
        temp.clear();
    }

}

void SMMTleverARM::distanc()
{
    vector<double> temp;
    double temp2;

    for (unsigned int i = 0; i < data.size() ; i++)
    {

        for (unsigned int j = 0; j < data[i].size(); j++)
        {

            temp2 = stod(data[i][j].substr(47,10));
//            cout<<temp2<<endl;

            temp.push_back(temp2);
        }
        dists.push_back(temp);
        temp.clear();
    }

}

topoPoint::topoPoint(double h,double z,double di,bool isRad)
{
    Hz=h;
    Ze=z;
    Di=di;
    rad = isRad;
    cart();
}

void topoPoint::cart()
{
    double X,Y,Z;

    if (rad)
    {
        X = std::sin(Ze) * std::sin(Hz) * Di;
        Y = std::sin(Ze) * std::cos(Hz) * Di;
        Z = std::cos(Ze) * Di;
    }
    else
    {
        X = std::sin(Ze*(datum::pi/180)) * std::sin(Hz*(datum::pi/180)) * Di;
        Y = std::sin(Ze*(datum::pi/180)) * std::cos(Hz*(datum::pi/180)) * Di;
        Z = std::cos(Ze*(datum::pi/180)) * Di;
    }

    vec3 temp = {X,Y,Z};

    XYZ = temp;
}

double pdpiH(double h1,double h2)
{
    if (h1 > h2)
    {
        return (h1+h2+180)/2;
    }
    else
    {
        return (h1+h2-180)/2;
    }
}

double pdpiV(double v1,double v2)
{
    if (v1 < v2)
    {
        return (v1-v2+360)/2;
    }
    else
    {
        return (v2-v1+360)/2;
    }
}

vec4 plane3points(vec3 u,vec3 v,vec3 w)
{
//the function returns a 4-vector with the plane equation coefficients
//assuming the form  ax + by + cz + d = 0
    vec4 res;
    vec3 p1,p2,n;
    vec d;
    p1 = normalise(v - u);
    p2 = normalise(w - u);
    n  = normalise(cross(p1,p2));
    d = - dot(n,u);

    res = join_vert(n,d);

//cout << res <<endl;
    return res;
}

void topoPoint::translate(vec3 tvec)
{
    XYZ += tvec;
}

void topoPoint::Rotate(mat R)
{
    XYZ = R * XYZ;
}

void TptMat (mat *M,vec Tvec)
{
    mat temp = *M;

    if(temp.n_cols == Tvec.n_elem)
    {
        for (uword i = 0; i < temp.n_rows; i++)
        {
            temp.row(i) = trans(trans(temp.row(i)) + Tvec );
        }
    }
    else
    {
        cout << "nada realizado, tamanho de colunas da matriz diferente do numero de elementos do vetor dado"<<endl;
    }

    *M = temp;
}

void RptMat (mat *M,mat R)
{
    mat temp = *M;

    if( (temp.n_cols == R.n_cols) & (R.n_cols == R.n_rows) )
    {
        for (uword i = 0; i < temp.n_rows; i++)
        {
            temp.row(i) = trans(R*trans(temp.row(i)));
        }
    }
    else
    {
        cout << "nada realizado, tamanho de colunas da matriz diferente do numero de colunas da matriz dada"<<endl;
        cout<<"ou a matriz fornecida não é quadrada"<<endl;
    }

    *M = temp;
}

void SMMTleverARM::translateAll(vec3 tvec)
{
    for (unsigned int i = 0; i < points.size(); i++)
    {
        for (unsigned int j = 0; j < points[i].size(); j++)
        {
            points[i][j].translate(tvec);
        }
    }
}

void SMMTleverARM::rotAll (mat R)
{
    for (unsigned int i = 0; i < points.size(); i++)
    {
        for (unsigned int j = 0; j < points[i].size(); j++)
        {
            points[i][j].Rotate(R);
        }
    }
}

mat topoPointsMat (vector<topoPoint> points,uword first,uword last)
{
    mat res;

    uword nlin = last - first + 1;

    res.zeros(nlin,3);

    for (uword i = first; i < last+1; i++)
    {
        res.row(i-first) = trans(points.at(i).XYZ);
//        cout << i <<endl;
    }

//    cout<<res<<endl;

    return res;
}

vec4 parallelPlane(vec4 plane1,vec3 pointOnPlane)
{
    vec4 res;
    vec3 n = normalise(plane1.rows(0,2));

    res.rows(0,2) = n;
    res(3) = - dot(n,pointOnPlane);

    return res;
}

vec3 ProjPtOrtPlane(vec3 pt2Proj,vec3 plNorm,vec3 ptOnPlane)
{
    return pt2Proj + (dot(normalise(plNorm),ptOnPlane-pt2Proj)) * normalise(plNorm);
}

vec3 onPlanePoint(vec4 plCoef,double x,double y)
{
    vec3 res;

    res(0) = x;
    res(1) = y;
    res(2) = - (plCoef(0)*x+plCoef(1)*y+plCoef(3))/plCoef(2);

    return res;
}

void SMMTleverARM::report()
{
    ofstream out("report.txt");
    out.precision(8);

    out << "Relatório de Saída do Processamento da ";
    out<< "determinação dos parâmetros de orientação relativa"   <<endl;
    out<<"do Sistema de Mapeamento Móvel Terrestre do LAPE"<<endl<<endl<<endl;

    out << "Lever Arm (no body-frame da IMU) --------------------------------"<<endl<<endl;

    out<< "IMU -> Antena"<<endl;
    out << "X(m): " <<antLA(0)<< " | Y(m): " <<antLA(1)<< " | Z(m): " <<antLA(2)<<endl<<endl;

    out<< "IMU -> Câmera Esquerda"<<endl;
    out << "X(m): " <<LcamLA(0)<< " | Y(m): " <<LcamLA(1)<< " | Z(m): " <<LcamLA(2)<<endl<<endl;

    out<< "IMU -> Câmera Direita"<<endl;
    out << "X(m): " <<RcamLA(0)<< " | Y(m): " <<RcamLA(1)<< " | Z(m): " <<RcamLA(2)<<endl<<endl<<endl;

    out<<"Distancias:"<<endl;
    out<<"Entre câmeras: "<<arma::norm(LcamLA-RcamLA)<<endl;
    out<<"IMU - Antena: "<<arma::norm(antLA)<<endl;
    out<<"IMU - Cam Esq. : "<<arma::norm(LcamLA)<<endl;
    out<<"IMU - Cam Dir. : "<<arma::norm(RcamLA)<<endl;

    out<< "Matrizes do Boresight (cossenos diretores): "<<endl<<endl;
    Rimu_LC.raw_print(out,"IMU (BF) -> Câmera Esquerda (BF)");
    out<<endl;
    Rimu_RC.raw_print(out,"IMU (BF) -> Câmera Direita (BF)");
    out<<endl<<endl;

    out<< "Matrizes do Boresight (cossenos diretores, mesma convenção da IMU): "<<endl<<endl;
    Rimu_LC2.raw_print(out,"IMU (BF) -> Câmera Esquerda (BF)");
    out<<endl;
    Rimu_RC2.raw_print(out,"IMU (BF) -> Câmera Direita (BF)");
    out<<endl<<endl;

    out<<"no último caso, os angulos formados com os eixos do BF da IMU: "<<endl<<endl;

    out<<"IMU (BF) -> Câmera Esquerda (BF)"<<endl<<arma::acos(Rimu_LC2)*(180/datum::pi)<<endl<<endl;
    out<<"IMU (BF) -> Câmera Direita (BF)"<<endl<<arma::acos(Rimu_RC2)*(180/datum::pi)<<endl;

    out<<endl<<"testes de consistência dos resultados"<<endl;
    out<<"as matrizes de rotação deverão, ao ser multiplicadas por sua transposta, serem iguais a matriz identidade"<<endl<<endl;

    out<<"Rtopo_imu * Rtopo_imu.t()"<<endl<<Rot1 * Rot1.t()<<endl;

    out<<"Rimu_LC * Rimu_LC.t()"<<endl<<Rimu_LC * Rimu_LC.t()<<endl;
    out<<"Rimu_RC * Rimu_RC.t()"<<endl<<Rimu_RC * Rimu_RC.t()<<endl;

    out<<"Rimu_LC2 * Rimu_LC2.t()"<<endl<<Rimu_LC2 * Rimu_LC2.t()<<endl;
    out<<"Rimu_RC2 * Rimu_RC2.t()"<<endl<<Rimu_RC2 * Rimu_RC2.t()<<endl;

}


#ifndef YANNQ_HYPERCUBECONVLAYER_HH
#define YANNQ_HYPERCUBECONVLAYER_HH

#include <time.h>
#include <Eigen/Dense>
#include <algorithm>
#include <complex>
#include <fstream>
#include <memory>
#include <random>
#include <vector>

//oneDnn
#include <mkldnn.hpp>
#include <dnnl.hpp>

//Yannq
#include <Utilities/Utility.hpp>
#include <Utilities/Exceptions.hpp>
#include "AbstractLayer.hpp"
#include "Machines/DNNEngine.hpp"

namespace yannq {
/** Convolutional layer with spin 1/2 hidden units.
 Important: In order for this to work correctly, Vector and Matrix must
 be column major.
 */

template<typename T>
class Conv1D;

template<>
class Conv1D<float>
	: public AbstractLayer<float>
{
public:
	using T = float;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

private:
	const bool useBias_;  // boolean to turn or off bias
	const uint32_t stride_;         // convolution stride
	const uint32_t npar_;          // number of parameters in layer

	dnnl::memory::dims weightDims_; //dimension of weights OC x IC x W
	dnnl::memory::desc weightMd_; //memory description
	dnnl::memory weight_;

	dnnl::memory::dims biasDims_; //dimension of bias OC
	dnnl::memory::desc biasMd_; //memory description
	dnnl::memory bias_;

	const dnnl::memory::data_type dtype = dnnl::memory::data_type::f32;

	static uint32_t numParams(bool useBias, const uint32_t inChannels,
			const uint32_t outChannels, const uint32_t kernelSize)
	{
		uint32_t np = inChannels * outChannels * kernelSize;
		if(useBias)
			np += outChannels;
		return np;
	}

public:
	/// Constructor
	Conv1D(	const uint32_t inChannels, const uint32_t outChannels,
			const uint32_t kernelSize, const uint32_t stride = 1,
			const bool useBias = false)
		: useBias_(useBias), stride_(stride),
		npar_(numParams(useBias, inChannels, outChannels, kernelSize)),
		weightDims_{outChannels, inChannels, kernelSize},
		biasDims_{outChannels}
	{
		using namespace dnnl;

		auto& engine = DNNEngine::getEngine();

		weightMd_ = memory::desc({weightDims_}, dtype, memory::format_tag::oiw);
		weight_ = memory(weightMd_, engine);

		biasMd_ = memory::desc({biasDims_}, dtype, memory::format_tag::a);
		bias_ = memory(biasMd_, engine);

		if(!useBias_)
		{
			T* p = static_cast<T*>(bias_.get_data_handle());
			for(int i = 0; i < biasDims_[0]; ++i)
			{
				p[i] = T(0.0);
			}
		}
	}

	Conv1D(const Conv1D& rhs) = default;
	Conv1D(Conv1D&& rhs) = default;

	Conv1D& operator=(const Conv1D& rhs) = delete;

	Conv1D& operator=(Conv1D&& rhs) = delete;

	bool operator==(const Conv1D& rhs) const
	{
		bool sameShape = (useBias_ == rhs.useBias_) &&
			(weightDims_ == rhs.weightDims_) &&
			(stride_ == rhs.stride_);

		if(!sameShape)
			return false;

		bool sameWeight = (std::memcmp(weight_.get_data_handle(),
					rhs.weight_.get_data_handle(),
					sizeof(T)*weightSize()) == 0);

		if(!useBias_)
			return sameWeight;

		if(sameWeight)
			return std::memcmp(bias_.get_data_handle(),
					rhs.bias_.get_data_handle(),
					sizeof(T)*biasDims_[0]);

		return false;
	}

	uint32_t weightSize() const
	{
		uint32_t prod = 1;
		for(auto dim: weightDims_)
			prod *= dim;
		return prod;
	}

	std::string name() const override { return "Convolutional 1D Layer"; }

	template<class RandomEngine>
	void randomizeParams(RandomEngine&& re, double sigma)
	{
		setParams(randomVector<T>(std::forward<RandomEngine>(re), sigma, npar_));
	}

	uint32_t paramDim() const override { return npar_; }

	uint32_t outputDim(uint32_t inputDim) const override
	{
		return (inputDim / stride_ / weightDims_[1]) * weightDims_[0];
	}

	Vector getParams() const override
	{
		Vector pars(npar_);
		pars.head(weightSize()) =
			Eigen::Map<const Vector>((const T*)weight_.get_data_handle(),
					weightSize());
		if(useBias_)
		{
			pars.tail(biasDims_[0]) =
				Eigen::Map<const Vector>((const T*)bias_.get_data_handle(),
						biasDims_[0]);
		}
		return pars;
	}

	void setParams(VectorConstRef pars) override
	{
		Eigen::Map<Vector>((T*)weight_.get_data_handle()) =
			pars.head(weightSize());
		if(useBias_)
		{
			Eigen::Map<Vector>((T*)bias_.get_data_handle(), biasDims_[0]) =
				pars.tail(biasDims_[0]);
		}
	}

	void updateParams(VectorConstRef ups) override
	{
		Eigen::Map<Vector>((T*)weight_.get_data_handle()) +=
			ups.head(weightSize());
		if(useBias_)
		{
			Eigen::Map<Vector>((T*)bias_.get_data_handle(), biasDims_[0]) +=
				ups.tail(biasDims_[0]);
		}
	}

	/**
	 * Feedforward for batch 1
	 * @param input inChannels*size
	 * @param output outChannels*size
	 */
	void forward(const VectorConstRef& input, VectorRef output) override
	{
		using namespace dnnl;
		assert(input.size() % inChannels_ == 0);

		uint32_t inSize = input.size() / weightDims_[1];
		uint32_t outSize = inSize / stride_;

		output.setZero();

		auto& engine = DNNEngine::getEngine();

		auto srcMd = memory::desc({1, weightDims_[1] /*inChannel*/,
				inSize}, dtype, memory::format_tag::ncw);
		auto src = memory(srcMd, engine);

		auto dstMd =
	}

	void backprop(const VectorConstRef& prev_layer_output,
			const VectorConstRef& this_layer_output,
			const VectorConstRef& dout,
			VectorRef din,
			VectorRef der) override
	{
		assert(prev_layer_output.size() % inChannels_ == 0);
		uint32_t inSize = prev_layer_output.size() / inChannels_;
		uint32_t outSize = inSize / stride_;

		din.resize(inSize*inChannels_);
		din.setZero();
		der.setZero();

		// propagate delta to prev-layer
		for (uint32_t oc = 0; oc < outChannels_; oc++)
		for (uint32_t r = 0; r < outSize; r ++)
		for (uint32_t ic = 0; ic < inChannels_; ic++)
		for (uint32_t ki = 0; ki < kernelSize_; ki++)
		{
			din(((r*stride_+ki-kernelSize_/2+inSize)%inSize) + ic*inSize)
				+= kernel_(ki + ic*kernelSize_, oc)*dout[r + oc*outSize];
		}

		uint32_t k = 0;
		if(useBias_)// accumulate bias difference
		{
			for (uint32_t oc = 0; oc < outChannels_; oc++)
			for (uint32_t r = 0; r < outSize; r ++)
			{
				der(oc) += dout(r + oc*outSize);
			}
			k += outChannels_;
		}

		Matrix dw(inChannels_*kernelSize_, outChannels_);
		dw.setZero();

		// accumulate weight difference
		for (uint32_t oc = 0; oc < outChannels_; oc++)
		for (uint32_t r = 0; r < outSize; r ++)
		{
			for (uint32_t ic = 0; ic < inChannels_; ic++)
			for (uint32_t ki = 0; ki < kernelSize_; ki++)
			{
				dw(ki + ic*kernelSize_, oc) +=
					prev_layer_output(((r*stride_+ki-kernelSize_/2+inSize)%inSize) + ic*inSize)*
					dout[r + oc*outSize];
			}
		}
		dw.resize(dw.rows()*dw.cols(),1);
		der.segment(k, kernel_.rows()*kernel_.cols()) = std::move(dw);
	}

	uint32_t fanIn() override
	{
		return inChannels_*kernelSize_;
	}
	uint32_t fanOut() override
	{
		return outChannels_*kernelSize_;
	}

	nlohmann::json desc() const override {
		nlohmann::json layerpar;
		layerpar["name"] = name();
		layerpar["use_bias"] = useBias_;
		layerpar["input_channels"] = inChannels_;
		layerpar["output_channels"] = outChannels_;
		layerpar["kernel_size"] = kernelSize_;
		layerpar["stride"] = stride_;
		return layerpar;
	}
};

template<typename T>
class Conv1D
	: public AbstractLayer<T>
{
	static_assert(!AbstractLayer<T>::Matrix::IsRowMajor, "Matrix must be column-major");

public:
	using Scalar = T;
	using Vector = Eigen::Matrix<T, Eigen::Dynamic, 1>;
	using Matrix = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>;
	using VectorRef = Eigen::Ref<Vector>;
	using VectorConstRef = Eigen::Ref<const Vector>;

private:
	const bool useBias_;  // boolean to turn or off bias

	const uint32_t inChannels_;   // number of input channels
	const uint32_t outChannels_;  // number of output channels

	const uint32_t kernelSize_;
	const uint32_t stride_;         // convolution stride

	const uint32_t npar_;          // number of parameters in layer

	Matrix kernel_;  // Weight parameters, W((inChannels_ * kernelSize)x(outChannels))
	Vector bias_;     // Bias parameters, b(outChannels)

	static uint32_t numParams(bool useBias, const uint32_t inChannels,
			const uint32_t outChannels, const uint32_t kernelSize)
	{
		uint32_t np = inChannels * outChannels * kernelSize;
		if(useBias)
			np += outChannels;
		return np;
	}

public:
	/// Constructor
	Conv1D(	const uint32_t inChannels, const uint32_t outChannels,
			const uint32_t kernelSize, const uint32_t stride = 1,
			const bool useBias = false)
		: useBias_(useBias), inChannels_(inChannels), outChannels_(outChannels),
		kernelSize_(kernelSize), stride_(stride),
		npar_(numParams(useBias, inChannels, outChannels, kernelSize)),
		kernel_(inChannels*kernelSize, outChannels), bias_(outChannels)
	{
	}

	Conv1D(const Conv1D& rhs) = default;
	Conv1D(Conv1D&& rhs) = default;

	Conv1D& operator=(const Conv1D& rhs) = default;
	Conv1D& operator=(Conv1D&& rhs) = default;

	bool operator==(const Conv1D& rhs) const
	{
		if(useBias_ != rhs.useBias_)
			return false;

		bool res = (inChannels_ == rhs.inChannels_) &&
				(outChannels_ == rhs.outChannels_) &&
				(kernelSize_ == rhs.kernelSize_) &&
				(stride_ == rhs.stride_) &&
				(kernel_ == rhs.kernel_);

		if(!useBias_)
			return res;
		else
			return res && (bias_ == rhs.bias_);
	}

	std::string name() const override { return "Convolutional 1D Layer"; }

	template<class RandomEngine>
	void randomizeParams(RandomEngine&& re, remove_complex_t<Scalar> sigma)
	{
		setParams(randomVector<T>(std::forward<RandomEngine>(re), sigma, npar_));
	}

	uint32_t paramDim() const override { return npar_; }
	uint32_t outputDim(uint32_t inputDim) const override {
		return (inputDim / stride_ / inChannels_) * outChannels_;
	}

	Vector getParams() const override
	{
		Vector pars(npar_);
		pars.head(kernel_.size()) = Eigen::Map<const Vector>(kernel_.data(), kernel_.size());
		if(useBias_)
		{
			pars.tail(outChannels_) = bias_;
		}
		return pars;
	}

	void setParams(VectorConstRef pars) override
	{
		Eigen::Map<Vector>(kernel_.data(), kernel_.size()) = pars.head(kernel_.size());
		if(useBias_)
		{
			bias_ = pars.segment(kernel_.size(), outChannels_);
		}
		else
	}

	void updateParams(VectorConstRef ups) override
	{
		Eigen::Map<Vector>(kernel_.data(), kernel_.size()) += ups.head(kernel_.size());
		if(useBias_)
		{
			bias_ += ups.segment(kernel_.size(), outChannels_);
		}
	}

	uint32_t fanIn() override
	{
		return inChannels_*kernelSize_;
	}

	uint32_t fanOut() override
	{
		return outChannels_*kernelSize_;
	}

	nlohmann::json desc() const override {
		nlohmann::json layerpar;
		layerpar["name"] = name();
		layerpar["use_bias"] = useBias_;
		layerpar["input_channels"] = inChannels_;
		layerpar["output_channels"] = outChannels_;
		layerpar["kernel_size"] = kernelSize_;
		layerpar["stride"] = stride_;
		return layerpar;
	}
};
}// namespace yannq

#endif


/**
 * \file ChamberlinFilter.cpp
 */

#include "ChamberlinFilter.h"

#include <boost/math/constants/constants.hpp>

namespace ATK
{
  template<typename DataType>
  ChamberlinFilter<DataType>::ChamberlinFilter()
  :TypedBaseFilter<DataType>(1, 1), numerical_frequency(0), numerical_attenuation(1), yh(0), yb(0), yl(0), selected(0), attenuation(1), cutoff_frequency(0)
  {
  }

  template<typename DataType_>
  void ChamberlinFilter<DataType_>::set_cut_frequency(DataType_ cutoff_frequency)
  {
    this->cutoff_frequency = cutoff_frequency;
    setup();
  }

  template<typename DataType_>
  DataType_ ChamberlinFilter<DataType_>::get_cut_frequency() const
  {
    return cutoff_frequency;
  }

  template<typename DataType_>
  void ChamberlinFilter<DataType_>::set_attenuation(DataType_ attenuation)
  {
    this->attenuation = attenuation;
    setup();
  }

  template<typename DataType_>
  DataType_ ChamberlinFilter<DataType_>::get_attenuation() const
  {
    return attenuation;
  }

  template<typename DataType>
  void ChamberlinFilter<DataType>::select(int selection)
  {
    this->selected = selection;
  }

  template<typename DataType>
  int ChamberlinFilter<DataType>::get_selected() const
  {
    return selected;
  }

  template<typename DataType>
  void ChamberlinFilter<DataType>::setup()
  {
    numerical_frequency = 2 * std::sin(boost::math::constants::pi<DataType>() * cutoff_frequency / input_sampling_rate);
    numerical_attenuation = 2 * attenuation;
  }

  template<typename DataType>
  void ChamberlinFilter<DataType>::process_impl(int64_t size) const
  {
    const DataType* ATK_RESTRICT input = converted_inputs[0];
    DataType* ATK_RESTRICT output = outputs[0];
    for(int64_t i = 0; i < size; ++i)
    {
      yh = input[i] - yl - numerical_attenuation * yb;
      yb = numerical_frequency * yh + yb;
      yl = numerical_frequency * yb + yl;
      if(selected == 0)
      {
        output[i] = yl;
      }
      else if(selected == 1)
      {
        output[i] = yb;
      }
      else
      {
        output[i] = yh;
      }
    }
  }

  template class ChamberlinFilter<float>;
  template class ChamberlinFilter<double>;
}


/*
 * Bayes++ the Bayesian Filtering Library
 * Copyright (c) 2002 Michael Stevens
 * See accompanying Bayes++.htm for terms and conditions of use.
 *
 * $Id$
 */

/*
 * Good random numbers from Boost
 *  Provides a common class  for all random number requirements to test Bayes++
 */

#include <boost/version.hpp>
#include <boost/random.hpp>

namespace Bayesian_filter_test
{

namespace
{
	template<class Engine, class Distribution>
	class simple_generator
	{
	public:
		typedef typename Distribution::result_type result_type;
		simple_generator(Engine& e, Distribution& d)
			: _eng(e), _dist(d)
		{}
		result_type operator()()
		{	return _dist(_eng);
		 }
	private:
		Engine& _eng;
		Distribution& _dist;
	};
}//namespace

class Boost_random
{
public:
	typedef Bayesian_filter_matrix::Float Float;
	typedef boost::mt19937 URng;
	Boost_random() : dist_uniform_01(), dist_normal_01()
	{}
	Bayesian_filter_matrix::Float normal(const Float mean, const Float sigma)
	{
		boost::normal_distribution<Float> dist(mean, sigma);
		return dist(rng);
	}
	void normal(Bayesian_filter_matrix::DenseVec& v, const Float mean, const Float sigma)
	{
		boost::normal_distribution<Float> dist(mean, sigma);
		simple_generator<URng, boost::normal_distribution<Float> > gen(rng, dist);
		std::generate (v.begin(), v.end(), gen);
	}
	void normal(Bayesian_filter_matrix::DenseVec& v)
	{
		simple_generator<URng, boost::normal_distribution<Float> > gen(rng, dist_normal_01);
		std::generate (v.begin(), v.end(), gen);
	}
	void uniform_01(Bayesian_filter_matrix::DenseVec& v)
	{
		simple_generator<URng, boost::uniform_01<Float> > gen(rng, dist_uniform_01);
		std::generate (v.begin(), v.end(), gen);
	}
#ifdef BAYES_FILTER_GAPPY
	void normal(Bayesian_filter_matrix::Vec& v, const Float mean, const Float sigma)
	{
		boost::normal_distribution<Float> dist(mean, sigma);
		simple_generator_01<URng, boost::normal_distribution<Float> > gen(rng, dist);
		for (std::size_t i = 0, iend=v.size(); i < iend; ++i)
			v[i] = gen();
	}
	void normal(Bayesian_filter_matrix::Vec& v)
	{
		simple_generator_01<URng, boost::normal_distribution<Float> > gen(rng, dist_normal_01);
		for (std::size_t i = 0, iend=v.size(); i < iend; ++i)
			v[i] = gen();
	}
	void uniform_01(Bayesian_filter_matrix::Vec& v)
	{
		simple_generator<URng, boost::uniform_01<Float> > gen(rng, dist_uniform_01);
		for (std::size_t i = 0, iend=v.size(); i < iend; ++i)
			v[i] = gen();
	}
#endif
	void seed()
	{
		rng.seed();
	}
private:
	URng rng;
	boost::uniform_01<Float> dist_uniform_01;
	boost::normal_distribution<Float> dist_normal_01;
};

}//namespace


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#include <iostream>
#include <sstream>

#include <geometry_test_common.hpp>

#include <boost/geometry/algorithms/equals.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/algorithms/transform.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/variant/variant.hpp>

#include <test_common/test_point.hpp>

template <typename Geometry1, typename Geometry2>
void check_transform(Geometry1 const& geometry1,
                     Geometry2 const& expected)
{
    Geometry2 geometry2;
    BOOST_CHECK(bg::transform(geometry1, geometry2));

    std::ostringstream result_wkt, expected_wkt;
    result_wkt << bg::wkt(geometry2);
    expected_wkt << bg::wkt(expected);
    BOOST_CHECK_EQUAL(result_wkt.str(), expected_wkt.str());
}

template <typename P1, typename P2, typename Value>
void test_transform_point(Value value)
{
    P1 p1;
    bg::set<0>(p1, 1);
    bg::set<1>(p1, 2);
    boost::variant<P1> v(p1);

    P2 expected;
    bg::assign(expected, p1);
    bg::multiply_value(expected, value);

    check_transform(p1, expected);
    check_transform(v, expected);
}

template <typename P1, typename P2, typename Value>
void test_transform_linestring(Value value)
{
    typedef bg::model::linestring<P1> line1_type;
    typedef bg::model::linestring<P2> line2_type;

    line1_type line1;
    line1.push_back(bg::make<P1>(1, 1));
    line1.push_back(bg::make<P1>(2, 2));
    boost::variant<line1_type> v(line1);

    line2_type expected;
    for (BOOST_AUTO(p, line1.begin()); p != line1.end(); ++p)
    {
        P2 new_point;
        bg::assign(new_point, *p);
        bg::multiply_value(new_point, value);
        expected.push_back(new_point);
    }

    check_transform(line1, expected);
    check_transform(v, expected);
}

template <typename P1, typename P2, typename Value>
void test_all(Value value)
{
    test_transform_point<P1, P2>(value);
    test_transform_linestring<P1, P2>(value);
}

template <typename T, typename DegreeOrRadian>
void test_transformations(double phi, double theta, double r)
{
    typedef bg::model::point<T, 3, bg::cs::cartesian> cartesian_type;
    cartesian_type p;

    // 1: using spherical coordinates
    {
        typedef bg::model::point<T, 3, bg::cs::spherical<DegreeOrRadian> >  spherical_type;
        spherical_type sph1;
        assign_values(sph1, phi, theta, r);
        BOOST_CHECK(transform(sph1, p));

        spherical_type sph2;
        BOOST_CHECK(transform(p, sph2));

        BOOST_CHECK_CLOSE(bg::get<0>(sph1), bg::get<0>(sph2), 0.001);
        BOOST_CHECK_CLOSE(bg::get<1>(sph1), bg::get<1>(sph2), 0.001);
    }

    // 2: using spherical coordinates on unit sphere
    {
        typedef bg::model::point<T, 2, bg::cs::spherical<DegreeOrRadian> >  spherical_type;
        spherical_type sph1, sph2;
        assign_values(sph1, phi, theta);
        BOOST_CHECK(transform(sph1, p));
        BOOST_CHECK(transform(p, sph2));

        BOOST_CHECK_CLOSE(bg::get<0>(sph1), bg::get<0>(sph2), 0.001);
        BOOST_CHECK_CLOSE(bg::get<1>(sph1), bg::get<1>(sph2), 0.001);
    }
}

int test_main(int, char* [])
{
    typedef bg::model::d2::point_xy<double > P;
    test_all<P, P>(1.0);
    test_all<bg::model::d2::point_xy<int>, bg::model::d2::point_xy<float> >(1.0);

    test_all<bg::model::point<double, 2, bg::cs::spherical<bg::degree> >,
        bg::model::point<double, 2, bg::cs::spherical<bg::radian> > >(bg::math::d2r);
    test_all<bg::model::point<double, 2, bg::cs::spherical<bg::radian> >,
        bg::model::point<double, 2, bg::cs::spherical<bg::degree> > >(bg::math::r2d);

    test_all<bg::model::point<int, 2, bg::cs::spherical<bg::degree> >,
        bg::model::point<float, 2, bg::cs::spherical<bg::radian> > >(bg::math::d2r);

    test_transformations<float, bg::degree>(4, 52, 1);
    test_transformations<double, bg::degree>(4, 52, 1);

    test_transformations<float, bg::radian>(3 * bg::math::d2r, 51 * bg::math::d2r, 1);
    test_transformations<double, bg::radian>(3 * bg::math::d2r, 51 * bg::math::d2r, 1);

#if defined(HAVE_TTMATH)
    typedef bg::model::d2::point_xy<ttmath_big > PT;
    test_all<PT, PT>();
    test_transformations<ttmath_big, bg::degree>(4, 52, 1);
    test_transformations<ttmath_big, bg::radian>(3 * bg::math::d2r, 51 * bg::math::d2r, 1);
#endif

    return 0;
}


/* ----------------------------------------------------------------------------

 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
 * Atlanta, Georgia 30332-0415
 * All Rights Reserved
 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)

 * See LICENSE for the license information

 * -------------------------------------------------------------------------- */

/**
 * @file    testNonlinearFactorGraph.cpp
 * @brief   Unit tests for Non-Linear Factor NonlinearFactorGraph
 * @brief   testNonlinearFactorGraph
 * @author  Carlos Nieto
 * @author  Christian Potthast
 */

#include <gtsam/base/Testable.h>
#include <gtsam/base/Matrix.h>
#include <tests/smallExample.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/BetweenFactor.h>

#include <CppUnitLite/TestHarness.h>

#include <boost/assign/std/list.hpp>
#include <boost/assign/std/set.hpp>
using namespace boost::assign;

/*STL/C++*/
#include <iostream>

using namespace std;
using namespace gtsam;
using namespace example;

using symbol_shorthand::X;
using symbol_shorthand::L;

/* ************************************************************************* */
TEST( NonlinearFactorGraph, equals )
{
  NonlinearFactorGraph fg = createNonlinearFactorGraph();
  NonlinearFactorGraph fg2 = createNonlinearFactorGraph();
  CHECK( fg.equals(fg2) );
}

/* ************************************************************************* */
TEST( NonlinearFactorGraph, error )
{
  NonlinearFactorGraph fg = createNonlinearFactorGraph();
  Values c1 = createValues();
  double actual1 = fg.error(c1);
  DOUBLES_EQUAL( 0.0, actual1, 1e-9 );

  Values c2 = createNoisyValues();
  double actual2 = fg.error(c2);
  DOUBLES_EQUAL( 5.625, actual2, 1e-9 );
}

/* ************************************************************************* */
TEST( NonlinearFactorGraph, keys )
{
  NonlinearFactorGraph fg = createNonlinearFactorGraph();
  KeySet actual = fg.keys();
  LONGS_EQUAL(3, (long)actual.size());
  KeySet::const_iterator it = actual.begin();
  LONGS_EQUAL((long)L(1), (long)*(it++));
  LONGS_EQUAL((long)X(1), (long)*(it++));
  LONGS_EQUAL((long)X(2), (long)*(it++));
}

/* ************************************************************************* */
TEST( NonlinearFactorGraph, GET_ORDERING)
{
  Ordering expected; expected += L(1), X(2), X(1); // For starting with l1,x1,x2
  NonlinearFactorGraph nlfg = createNonlinearFactorGraph();
  Ordering actual = Ordering::Colamd(nlfg);
  EXPECT(assert_equal(expected,actual));

  // Constrained ordering - put x2 at the end
  Ordering expectedConstrained; expectedConstrained += L(1), X(1), X(2);
  FastMap<Key, int> constraints;
  constraints[X(2)] = 1;
  Ordering actualConstrained = Ordering::ColamdConstrained(nlfg, constraints);
  EXPECT(assert_equal(expectedConstrained, actualConstrained));
}

/* ************************************************************************* */
TEST( NonlinearFactorGraph, probPrime )
{
  NonlinearFactorGraph fg = createNonlinearFactorGraph();
  Values cfg = createValues();

  // evaluate the probability of the factor graph
  double actual = fg.probPrime(cfg);
  double expected = 1.0;
  DOUBLES_EQUAL(expected,actual,0);
}

/* ************************************************************************* */
TEST( NonlinearFactorGraph, linearize )
{
  NonlinearFactorGraph fg = createNonlinearFactorGraph();
  Values initial = createNoisyValues();
  GaussianFactorGraph linearFG = *fg.linearize(initial);
  GaussianFactorGraph expected = createGaussianFactorGraph();
  CHECK(assert_equal(expected,linearFG)); // Needs correct linearizations
}

/* ************************************************************************* */
TEST( NonlinearFactorGraph, clone )
{
  NonlinearFactorGraph fg = createNonlinearFactorGraph();
  NonlinearFactorGraph actClone = fg.clone();
  EXPECT(assert_equal(fg, actClone));
  for (size_t i=0; i<fg.size(); ++i)
    EXPECT(fg[i] != actClone[i]);
}

/* ************************************************************************* */
TEST( NonlinearFactorGraph, rekey )
{
  NonlinearFactorGraph init = createNonlinearFactorGraph();
  map<Key,Key> rekey_mapping;
  rekey_mapping.insert(make_pair(L(1), L(4)));
  NonlinearFactorGraph actRekey = init.rekey(rekey_mapping);

  // ensure deep clone
  LONGS_EQUAL((long)init.size(), (long)actRekey.size());
  for (size_t i=0; i<init.size(); ++i)
      EXPECT(init[i] != actRekey[i]);

  NonlinearFactorGraph expRekey;
  // original measurements
  expRekey.push_back(init[0]);
  expRekey.push_back(init[1]);

  // updated measurements
  Point2 z3(0, -1),  z4(-1.5, -1.);
  SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2);
  expRekey += simulated2D::Measurement(z3, sigma0_2, X(1), L(4));
  expRekey += simulated2D::Measurement(z4, sigma0_2, X(2), L(4));

  EXPECT(assert_equal(expRekey, actRekey));
}

/* ************************************************************************* */
TEST( NonlinearFactorGraph, symbolic )
{
  NonlinearFactorGraph graph = createNonlinearFactorGraph();

  SymbolicFactorGraph expected;
  expected.push_factor(X(1));
  expected.push_factor(X(1), X(2));
  expected.push_factor(X(1), L(1));
  expected.push_factor(X(2), L(1));

  SymbolicFactorGraph actual = *graph.symbolic();

  EXPECT(assert_equal(expected, actual));
}

/* ************************************************************************* */
TEST(NonlinearFactorGraph, UpdateCholesky) {
  NonlinearFactorGraph fg = createNonlinearFactorGraph();
  Values initial = createNoisyValues();

  // solve conventionally
  GaussianFactorGraph linearFG = *fg.linearize(initial);
  auto delta = linearFG.optimizeDensely();
  auto expected = initial.retract(delta);

  // solve with new method
  EXPECT(assert_equal(expected, fg.updateCholesky(initial)));

  // solve with Ordering
  Ordering ordering;
  ordering += L(1), X(2), X(1);
  EXPECT(assert_equal(expected, fg.updateCholesky(initial, ordering)));

  // solve with new method, heavily damped
  auto dampen = [](const HessianFactor::shared_ptr& hessianFactor) {
    auto iterator = hessianFactor->begin();
    for (; iterator != hessianFactor->end(); iterator++) {
      const auto index = std::distance(hessianFactor->begin(), iterator);
      auto block = hessianFactor->info().diagonalBlock(index);
      for (int j = 0; j < block.rows(); j++) {
        block(j, j) += 1e9;
      }
    }
  };
  EXPECT(assert_equal(initial, fg.updateCholesky(initial, dampen), 1e-6));
}

/* ************************************************************************* */
// Example from issue #452 which threw an ILS error. The reason was a very
// weak prior on heading, which was tightened, and the ILS disappeared.
TEST(testNonlinearFactorGraph, eliminate) {
  // Linearization point
  Pose2 T11(0, 0, 0);
  Pose2 T12(1, 0, 0);
  Pose2 T21(0, 1, 0);
  Pose2 T22(1, 1, 0);

  // Factor graph
  auto graph = NonlinearFactorGraph();

  // Priors
  auto prior = noiseModel::Isotropic::Sigma(3, 1);
  graph.addPrior(11, T11, prior);
  graph.addPrior(21, T21, prior);

  // Odometry
  auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.3));
  graph.add(BetweenFactor<Pose2>(11, 12, T11.between(T12), model));
  graph.add(BetweenFactor<Pose2>(21, 22, T21.between(T22), model));

  // Range factor
  auto model_rho = noiseModel::Isotropic::Sigma(1, 0.01);
  graph.add(RangeFactor<Pose2>(12, 22, 1.0, model_rho));

  Values values;
  values.insert(11, T11.retract(Vector3(0.1,0.2,0.3)));
  values.insert(12, T12);
  values.insert(21, T21);
  values.insert(22, T22);
  auto linearized = graph.linearize(values);

  // Eliminate
  Ordering ordering;
  ordering += 11, 21, 12, 22;
  auto bn = linearized->eliminateSequential(ordering);
  EXPECT_LONGS_EQUAL(4, bn->size());
}

/* ************************************************************************* */
TEST(testNonlinearFactorGraph, addPrior) {
  Key k(0);

  // Factor graph.
  auto graph = NonlinearFactorGraph();

  // Add a prior factor for key k.
  auto model_double = noiseModel::Isotropic::Sigma(1, 1);
  graph.addPrior<double>(k, 10, model_double);

  // Assert the graph has 0 error with the correct values.
  Values values;
  values.insert(k, 10.0);
  EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16);

  // Assert the graph has some error with incorrect values.
  values.clear();
  values.insert(k, 11.0);
  EXPECT(0 != graph.error(values));

  // Clear the factor graph and values.
  values.clear();
  graph.erase(graph.begin(), graph.end());

  // Add a Pose3 prior to the factor graph. Use a gaussian noise model by
  // providing the covariance matrix.
  Eigen::DiagonalMatrix<double, 6, 6> covariance_pose3;
  covariance_pose3.setIdentity();
  Pose3 pose{Rot3(), Point3(0, 0, 0)};
  graph.addPrior(k, pose, covariance_pose3);

  // Assert the graph has 0 error with the correct values.
  values.insert(k, pose);
  EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16);

  // Assert the graph has some error with incorrect values.
  values.clear();
  Pose3 pose_incorrect{Rot3::RzRyRx(-M_PI, M_PI, -M_PI / 8), Point3(1, 2, 3)};
  values.insert(k, pose_incorrect);
  EXPECT(0 != graph.error(values));
}

TEST(NonlinearFactorGraph, printErrors)
{
  const NonlinearFactorGraph fg = createNonlinearFactorGraph();
  const Values c = createValues();

  // Test that it builds with default parameters.
  // We cannot check the output since (at present) output is fixed to std::cout.
  fg.printErrors(c);

  // Second round: using callback filter to check that we actually visit all factors:
  std::vector<bool> visited;
  visited.assign(fg.size(), false);
  const auto testFilter =
      [&](const gtsam::Factor *f, double error, size_t index) {
        EXPECT(f!=nullptr);
        EXPECT(error>=.0);
        visited.at(index)=true;
        return false; // do not print
      };
  fg.printErrors(c,"Test graph: ", gtsam::DefaultKeyFormatter,testFilter);

  for (bool visit : visited) EXPECT(visit==true);
}

/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */


//////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::map_pdf::ratio_pdf::ratio_pdf.hpp                  //
//                                                                          //
//  (C) Copyright 2009 Erwann Rogard                                        //
//  Use, modification and distribution are subject to the                   //
//  Boost Software License, Version 1.0. (See accompanying file             //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)        //
//////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_RATIO_PDF_RATIO_PDF_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_MAP_PDF_RATIO_PDF_RATIO_PDF_HPP_ER_2009
#include <boost/call_traits.hpp>
#include <boost/statistics/detail/distribution_toolkit/map_pdf/inverse_pdf/inverse_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/map_pdf/product_pdf/product_pdf.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace distribution{
namespace toolkit{

    template<typename A,typename B>
    struct meta_ratio_pdf{
        typedef inverse_pdf<B> inv_;
        typedef product_pdf<A, inv_ > type;
        static type call(const A& a,const B& b){
            return type(
                a,
                inv_(b)
            );
         }
    };

    template<typename A,typename B>
    typename meta_ratio_pdf<A,B>::type
    make_ratio_pdf(const A& a, const B& b)
    {
        typedef meta_ratio_pdf<A,B> meta_;
        return meta_::call(a,b);
    }

}// distribution
}// toolkit
}// detail
}// statistics
}// boost

#endif


/*
 * Copyright (c) 2021, Tetsuro Nagai
 */

#include <iostream>
#include <fstream>
#include <vector>
#include <algorithm>
#include <numeric>
#include <cmath>
#include <cstring>
#include <cassert>
#include <string>
#include <iomanip>
#include <boost/program_options.hpp>
#include <boost/format.hpp>
#include <prettyprint.hpp>
#include "time_string.hpp"

constexpr double kB=1.380649e-23  ;
constexpr double NA=6.02214076e23 ;
constexpr double kB_kJ_per_mol=kB*NA/1000 ;
constexpr int BUF_MAX=100000;

constexpr int DIM_HIST=1 ;

using std::string;
using std::vector;
using std::array;
using std::cout;
using std::cerr;
using std::endl;
namespace po=boost::program_options;

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

  string starting_date;
  misc::put_string_of_time(starting_date);
  cout << "Execution start at " << starting_date <<endl;

  //get command line argments
  double min,max,dx, T;
  string fout_prefix;
  bool bStrict;

  // making command line options
  po::options_description opt("generic histogram and free energy. Data should be supplemented from the standard input; the first column will be analysed.");
  opt.add_options()
    ("help,h" ,                                                   "show help")
    ("dx"     ,      po::value<double>()->default_value(0.1),    "dx, width of histogram")
    ("min"     ,     po::value<double>()->default_value(-10),    "min of histogram")
    ("max"     ,     po::value<double>()->default_value(10),     "max of histogram")
    ("T"     ,       po::value<double>()->default_value(300),     "temperature for free energy convertion")
    ("bStrictOutOfRange"   , po::value<bool>()->default_value(false),     "if true, abort when a sample out of min and max is found")
    ("fout_prefix",  po::value<string>(),                         "fout_prefix, a number of files will be made with this prefix");

  // analyze argc and argv and results are stored in vm
  try{
    po::variables_map vm;
    store(parse_command_line(argc, argv, opt), vm);
    notify(vm);
    if(vm.count("help")){
      cout << opt << endl; // show help
      exit(1);
    }
    else if(!vm.count("fout_prefix")){
      cerr << vm.count("fout_prefix") << endl;
      cerr << "fout_prefix is mandatory " << endl;
      cerr << "exit!!" << endl;
      exit(1);
    }
    else
    {
      dx = vm["dx"].as<double>();
      min = vm["min"].as<double>();
      max = vm["max"].as<double>();
      T = vm["T"].as<double>();
      bStrict = vm["bStrictOutOfRange"].as<bool>();
      fout_prefix = vm["fout_prefix"].as<string>();
		}
	}
  catch (boost::bad_any_cast &e) {
    cout << e.what() << endl;
  	cout <<"something wrong and buggy happend!!"  << endl;
  	cout <<"exit!!"  << endl;
  	exit(1);
  }
  catch (std::exception  &e) {
    cout << e.what() << endl;
    cout <<"exit!!"  << endl;
    exit(2);
  }

  cout << "**** Input parameters ****"  << endl;
  cout << "dx: " << dx << endl;
  cout << "min: " << min << endl;
  cout << "max: " << max << endl;
  cout << "T: " << T << endl;
  cout << "bStrictOutOfRange: " << bStrict << endl;
  cout << "fout_prefix: " << fout_prefix << endl;
  cout << "*************************\n"  << endl;

	const string fname_hist = fout_prefix+"_hist.dat";
	const string fname_FE   = fout_prefix+"_free_energy.dat";

  cout << "Files to be created: " << fname_hist << endl;
  cout << "Files to be created: " << fname_FE  << endl;

  std::ofstream ofs_hist(fname_hist.c_str());
  std::ofstream ofs_FE(fname_FE.c_str());

  if(!ofs_hist){
    cerr << "cannot open " << fname_hist << endl;
    return -1;
  }
  if(!ofs_FE){
    cerr << "cannot open " << fname_FE << endl;
    return -1;
  }

  if(max<=min){
    cout << "ERROR: max < min" <<endl;
    cerr << "ERROR: max < min" <<endl;
    return -1;
  }

  const int  nbins = std::ceil((max-min)/dx);
  vector<double>  edges(nbins+1,0);
  vector<int>     counts(nbins);
  vector<double>  pdf(nbins);
  vector<double>  pdferr(nbins);
  vector<double>  free_energy(nbins);

  for (int i = 0 ; i < nbins+1; i++){
    edges[i] = min + dx*i ;
  }

  cout << "**** parameters determined ****"  << endl;
  cout << "nbins: " << nbins <<endl;
  cout << "edges: " << edges <<endl;
  cout << "*******************************"  << endl;

  double val;
  string buf;
  char tmp[BUF_MAX];
  char* err;

  auto get_ibin = [](double val,double min, double dx){return std::floor((val-min)/dx);};
  int ibin;

  while (std::getline(std::cin, buf))
  {
    auto bsscanf = std::sscanf(buf.c_str(), "%s" , tmp );
    if(bsscanf != DIM_HIST){
        cout << "ERROR: not enough columns\n" << "Exit!!" <<endl;
        cerr << "ERROR: not enough columns\n" << "Exit!!" <<endl;
        return -1;
    }

    val=std::strtod(tmp, &err);
    if(*err!='\0'){
      cout << "ERROR: error to convert: " << tmp << "\nExit!!" <<endl;
      cerr << "ERROR: error to convert: " << tmp << "\nExit!!" <<endl;
      //continue;
      return -1;
    }

    ibin = get_ibin(val, min, dx);
    if(val < min){
      cout << "min is too large: set min such that min < " << val << endl;
      cerr << "min is too large: set min such that min < " << val << endl;
      ibin = 0;
      if(bStrict){
        cout << "Abort at parsing: " << buf << endl;
        cerr << "Abort at parsing: " << buf << endl;
        return -1;
      }
    }
    if(val >= max){
      cout << "max is too small: set max such that max> " << val << endl;
      cerr << "max is too small: set max such that max> " << val << endl;
      ibin = nbins-1;
      if(bStrict){
        cout << "As bStrictOutOfRange is on, abort at parsing: " << buf << endl;
        cerr << "As bStrictOutOfRange is on, abort at parsing: " << buf << endl;
        return -1;
      }
    }
    counts[ibin]++;
  }

  int ncounts = std::accumulate(counts.begin(), counts.end(), 0);
  cout << boost::format("In total, %d samples have been considered.") % ncounts << endl;

  for(int i=0; i < nbins; i++){
    pdf[i] = counts[i]/double(ncounts)/dx;
    pdferr[i] = std::sqrt((counts[i]/double(ncounts))*(1.0-counts[i]/double(ncounts))/double(ncounts)); //based on binary dist
    pdferr[i] /= dx;
    free_energy[i] = -kB_kJ_per_mol*T*std::log(pdf[i]);
  }

  double min_free_energy = *std::min_element(free_energy.begin(),free_energy.end());

  for(auto  &v1: free_energy){
    v1 -= min_free_energy;
  }

  cout << counts << endl;
  cout << free_energy << endl;

  for(int i= 0; i<nbins;i++){
  ofs_hist << boost::format("%10.5f %12.6g %12.6g %12d\n") % (edges[i]+0.5*dx) % pdf[i]  %pdferr[i] %counts[i] ;
  ofs_FE << boost::format("%10.5f %12.6g\n") % (edges[i]+0.5*dx) % free_energy[i] ;
  }
  ofs_hist.close();
  ofs_FE.close();

  return EXIT_SUCCESS ;
}


#ifndef STAN_OPTIMIZATION_BFGS_UPDATE_HPP
#define STAN_OPTIMIZATION_BFGS_UPDATE_HPP

#include <Eigen/Dense>

namespace stan {
  namespace optimization {
    template<typename Scalar = double,
             int DimAtCompile = Eigen::Dynamic>
    class BFGSUpdate_HInv {
    public:
      typedef Eigen::Matrix<Scalar, DimAtCompile, 1> VectorT;
      typedef Eigen::Matrix<Scalar, DimAtCompile, DimAtCompile> HessianT;

      /**
       * Update the inverse Hessian approximation.
       *
       * @param yk Difference between the current and previous gradient vector.
       * @param sk Difference between the current and previous state vector.
       * @param reset Whether to reset the approximation, forgetting about
       * previous values.
       * @return In the case of a reset, returns the optimal scaling of the
       * initial Hessian approximation which is useful for predicting
       * step-sizes.
       **/
      inline Scalar update(const VectorT &yk, const VectorT &sk,
                           bool reset = false) {
        Scalar rhok, skyk, B0fact;
        HessianT Hupd;

        skyk = yk.dot(sk);
        rhok = 1.0/skyk;

        Hupd.noalias() = HessianT::Identity(yk.size(), yk.size())
                                        - rhok * sk * yk.transpose();
        if (reset) {
          B0fact = yk.squaredNorm()/skyk;
          _Hk.noalias() = ((1.0/B0fact)*Hupd)*Hupd.transpose();
        } else {
          B0fact = 1.0;
          _Hk = Hupd*_Hk*Hupd.transpose();
        }
        _Hk.noalias() += rhok*sk*sk.transpose();
        return B0fact;
      }

      /**
       * Compute the search direction based on the current (inverse) Hessian
       * approximation and given gradient.
       *
       * @param[out] pk The negative product of the inverse Hessian and gradient
       * direction gk.
       * @param[in] gk Gradient direction.
       **/
      inline void search_direction(VectorT &pk, const VectorT &gk) const {
        pk.noalias() = -(_Hk*gk);
      }

    private:
      HessianT _Hk;
    };
  }
}

#endif


#include <CGAL/point_generators_2.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_2.h>
#include <CGAL/Periodic_4_hyperbolic_Delaunay_triangulation_traits_2.h>
#include <CGAL/Hyperbolic_octagon_translation.h>
#include <CGAL/Cartesian.h>
#include <boost/tuple/tuple.hpp>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/uniform_smallint.hpp>
#include <boost/random/variate_generator.hpp>

#include <iostream>

typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_traits_2<>               Traits;
typedef CGAL::Periodic_4_hyperbolic_Delaunay_triangulation_2<Traits>                Triangulation;
typedef Triangulation::Face_iterator                                                Face_iterator;
typedef Triangulation::Vertex_handle                                                Vertex_handle;
typedef Triangulation::Point                                                        Point;
typedef Triangulation::Vertex_iterator                                              Iter;
typedef Traits::Side_of_original_octagon                                            Side_of_original_octagon;

typedef CGAL::Cartesian<double>                                                     DKernel;
typedef DKernel::Point_2                                                            DPoint;
typedef CGAL::Creator_uniform_2<double, DPoint >                                    Creator;

int main(int argc, char** argv)
{
  int N;
  if(argc < 2)
  {
    std::cout << "usage: " << argv[0] << " [number of points]" << std::endl;
    std::cout << "generating 1000 points (default)!" << std::endl;
    N = 1000;
  }
  else
  {
    N = atoi(argv[1]);
  }

  int iters = 1;
  if(argc == 3)
    iters = atoi(argv[2]);

  for(int itr = 0; itr < iters; ++itr)
  {
    Triangulation tr;

    CGAL::Random_points_in_disc_2<DPoint, Creator> g(0.85);
    Side_of_original_octagon pred;

    int cnt = 0;
    do {
      DPoint pt = *g;
      ++g;
      if(pred(pt) != CGAL::ON_UNBOUNDED_SIDE) {
        tr.insert(Point(pt.x(), pt.y()));
        cnt++;
      }
    }
    while(cnt < N);

    tr.try_to_remove_dummy_vertices();
    assert(tr.is_valid());

    cnt = 0;
    for(Iter it = tr.vertices_begin(); it != tr.vertices_end(); ++it) {
      tr.remove(it);
    }

    std::cout << "Final count of vertices: " << tr.number_of_vertices() << std::endl;
    assert(tr.is_valid());
  }

  return EXIT_SUCCESS;
}