#include <sys/time.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <vector>

#include "ros/ros.h"
#include "bosch_arm.h"
#include <kdl/frames.hpp>
#include "cartesian_controller2.h"
#include <bosch_arm_control/ArmState.h>
#include <bosch_arm_control/PointCmd.h>
#include <bosch_arm_control/Diagnostic.h>
#include <sensor_msgs/PointCloud.h>
#include<geometry_msgs/Point32.h>
#include "cc2.h"
#include <Eigen/Core>
#include<Eigen/Array>
#include <Eigen/LU>
#include <Eigen/QR>
#include <action_cart.h>

#include <sys/ioctl.h>
#include <fcntl.h>
#include <linux/kd.h>

using namespace std;
void CartesianController::singlePtCmdCallBack(const bosch_arm_control::PointCmd& msg)
{
    MoveAbsAction* a=new MoveAbsAction(msg.coordinates,this,msg.duration);
    act_que.push_back(a);
}

void CartesianController::GMSCallBack(const GMS120::GMS120_measurement& msg)
{

    tf::StampedTransform transform;
    transform.setOrigin( tf::Vector3(0.0, 0.0, 0.0) );
    transform.setRotation( tf::Quaternion(0, 0, 0) );
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "world"));

    pc.header.stamp = ros::Time::now();
    geometry_msgs::Point32 point;
    point.x=x[0];
    point.y=x[1];
    //point.z=x[2];
    //point.z=msg.capacitance_measured/2000.0;
    point.z=msg.inductance/300;
    if(msg.inductance>0)
    {
    pc.points.push_back(point);
    //pc.channels[0].values.push_back(msg.inductance);
    pc.channels[0].values.push_back(100);
//     if (msg.inductance>mu&&msg.inductance<last_inductance&&state!=SERVO)
//     {
//         state=SERVO;
//         char ascii=7;
//         cout<<"inductance:"<<msg.inductance<<", last inductance:"<<last_inductance<<". Stopped."<<endl;
//         //cout<<'\a'<<flush;
//
//     }
    last_inductance=msg.inductance;
    point_pub.publish(pc);
    }
    //cout<<msg.inductance<<endl;
}
CartesianController::CartesianController(BoschArm *ptr)
{
    rob=ptr;
    diagnostic_pub =  n.advertise<bosch_arm_control::Diagnostic> ("/diagnostics",100);
    point_pub=n.advertise<sensor_msgs::PointCloud>("/gmsdata",100);
    sigle_point_cmd_sub = n.subscribe("/joint_point_cmd",3, &CartesianController::singlePtCmdCallBack, this);
    GMS_sub = n.subscribe("/GMS120_measurements",3,&CartesianController::GMSCallBack,this);
    joint_state_pub =  n.advertise<bosch_arm_control::ArmState> ("/joint_states",100);
    js.position.resize(3);
    js.velocity.resize(3);
    js.effort.resize(3);

    pc.header.frame_id = "/world";
    pc.points.resize(0);
    pc.channels.resize(1);
    pc.channels[0].values.resize(0);
    pc.channels[0].name = "intensity";

    Kp[0]=80;
    Kp[1]=80;
    Kp[2]=80;

    Kc[0]=0.3;
    Kc[1]=0.3;
    Kc[2]=0.3;
    Kc[3]=0.3;
//    for(int i=0;i<4;i++)
//      Kp[i]=Kp[i]*0.02;
    Kv[0]=0.3;
    Kv[1]=0.3;
    Kv[2]=0.3;

    Ks[0]=0.05;
    Ks[1]=0.02;
    Ks[2]=0.30;
    mu=2.6;
//   for(int i=0;i<4;i++)
//     Kv[i]=0;
//   joint_limit_low[0]=-1.7;
//   joint_limit_high[0]=2.1;
//   joint_limit_low[1]=-0.6;
//   joint_limit_high[1]=0.55;
//   joint_limit_low[2]=-2.5;
//   joint_limit_high[2]=2.3;
//   joint_limit_low[3]=-2.3;
//   joint_limit_high[3]=2.4;
    cur_act=NULL;
    start_semi=false;
    end_semi=false;
    last_inductance=0;
    inductance_sum_last=0;
    cnta=0;
    n_cur=0;
    radius=0.04;
    cycle=4000;
    fd = open("/dev/console", O_WRONLY);
}

//calculate the frames given the joint values.
void CartesianController::updateKinematics()
{
    frm_cur[0]=frm_home[0];
    frm0[0]=frm_home[0];
    for (int i=1;i<=4;i++)
    {
        frm_cur[i].p=frm_home[i].p;
        frm_cur[i].M=frm_home[i].M*Rotation::RotZ(q[i-1]);
        frm0[i]=frm0[i-1]*frm_cur[i];
    }
    frm_cur[5]=frm_home[5];
    frm0[5]=frm0[4]*frm_cur[5];

    Vector z(0,0,1);
    Vector mx=frm0[5].M.UnitZ()*z;
    double norm=dot(mx,mx);
    if (norm<1e-4)
    {
        mx=z*frm0[5].M.UnitX();
        double n2=dot(mx,mx);
        mx=mx/sqrt(n2);
    }
    else
        mx=mx/sqrt(norm);
    Vector my=z*mx;

    frm0[6].M=Rotation(mx,my,z);

    frm0[6].p=frm0[6].M*scanner_p+frm0[5].p;

    frmn[4]=frm_cur[5];

    //jacobian[3](joint 4) = z4*p54
    for (int i=3;i>=0;i--)
    {
        frmn[i]=frm_cur[i+1]*frmn[i+1];
        jacobian[i]=z*frmn[i+1].p;
        jacobian[i]=frm0[i+1].M*jacobian[i];
    }
    Eigen::MatrixXf jcb(3,4);
    for (int i=0;i<3;i++)
        for (int j=0;j<4;j++)
            jcb(i,j)=jacobian[j][i];

    Eigen::Matrix3f mm=jcb*jcb.transpose();
    //cout<<mm<<endl;
    Eigen::Matrix3f inv=mm.inverse();
    //cout<<inv<<endl;
    nullspace=Eigen::Matrix4f::Identity()-jcb.transpose()*inv*jcb;
    //cout<<nullspace<<endl;
//
//   for(int i=0;i<3;i++)
//     for(int j=0;j<3;j++)
//     {
//       mm(i,j)=0;
//       for(int k=0;k<4;k++)
//         mm(i,j)=mm(i,j)+jacobian[k][i]*jacobian[k][j];
//     }
//   mm=mm.inverse();

}

void CartesianController::start()
{
    rob->initialize();

    for (int i=0;i<6;i++)
    {
        frm_home[i].p=Vector(constants::home_frame_p[i*3],
                             constants::home_frame_p[i*3+1],
                             constants::home_frame_p[i*3+2]);

        frm_home[i].M=Rotation(constants::home_frame_M[i*9],
                               constants::home_frame_M[i*9+1],
                               constants::home_frame_M[i*9+2],
                               constants::home_frame_M[i*9+3],
                               constants::home_frame_M[i*9+4],
                               constants::home_frame_M[i*9+5],
                               constants::home_frame_M[i*9+6],
                               constants::home_frame_M[i*9+7],
                               constants::home_frame_M[i*9+8]);
    }
    scanner_p=Vector(constants::scan_p[0],
                     constants::scan_p[1],
                     constants::scan_p[2]);

    for (int i=0;i<4;i++)
        gc[i]=Vector(constants::gravc[i*2],constants::gravc[i*2+1],0);

    rob->motor2JointPosition(rob->q,q);
    updateKinematics();
    for (int i=0;i<3;i++)
    {
        x[i]=frm0[5].p[i];
        v[i]=0;
        s[i]=0;
        dx[i]=0;
        f[i]=0;
        xl[i]=x[i];
        xd[i]=x[i];
        vd[i]=v[i];
    }
    //state=CALIBRATION;
    //state=SEMIAUTO;
    state=SERVO;
    ctr_mode=NOCONTROL;

    if (state==CALIBRATION)
    {
        genCalibTraj2();
        cur_act=act_que.front();
        cur_act->initialize();
        act_que.pop_front();
        cout<<"start in calibration"<<endl;
    }
    else if (state==SEMIAUTO)
    {
        vector<double> des(3,0);
        cali1_act=new MoveAbsAction(des,this);
        calib1=new Calibration("calibsemi");
        cali1_act->calib=calib1;
        act_que.push_back(cali1_act);
        cur_act=act_que.front();
        cur_act->initialize();
        act_que.pop_front();
        cout<<"start in semi auto"<<endl;
    }
    else if (state==LOADTRACE)
    {
//     for (int i=0;i<4;i++)
//       gc[i]=Vector(constants::gravc[i*2],constants::gravc[i*2+1],0);
//     vector<double> des(3,0);
//     des[0]=0.5;
//     des[1]=0;
//     des[2]=0.3;
//     act_que.push_back(new MoveAbsAction(des,this));
//     des[0]=0;
//     des[1]=0;
//     des[2]=1.2;
//     act_que.push_back(new MoveAbsAction(des,this));
//     genCalibTraj2();
//     cur_act=act_que.front();
//     cur_act->initialize();
//     act_que.pop_front();

        //state=SERVO;

    }
}

// void CartesianController::genCalibTraj2()
// {
//
//   vector<double> des(3,0);
// //  int t=8000;
//   //move the the zero position.
//   des[0]=0.30;
//   des[1]=-0.5;
//   des[2]=0.10;
//   act_que.push_back(new MoveAbsAction(des,this));
//   des[0]=0.19;
//   des[1]=-0.18;
//   des[2]=-0.05;
//   act_que.push_back(new MoveAbsAction(des,this));
//   des[0]=0.54;
//   des[1]=-0.18;
//   des[2]=-0.05;
//   cali1_act=new MoveAbsAction(des,this);
//   calib1=new Calibration("calib1");
//   cali1_act->calib=calib1;
//   act_que.push_back(cali1_act);
//   des[0]=0.54;
//   des[1]=0.07;
//   des[2]=-0.05;
//   cali2_act=new MoveAbsAction(des,this);
//   calib2=new Calibration("calib2");
//   cali2_act->calib=calib2;
//   act_que.push_back(cali2_act);
//
//
// }

void CartesianController::stopTrajectory()
{
    cout<<"stop scanning"<<endl;
    state=SERVO;
}

void CartesianController::genCalibTraj2()
{
    vector<double> des(3,0);
    while (act_que.size()!=0)
        act_que.pop_front();

    des[0]=0.5;
    des[1]=0.2;
    des[2]=0;
    cali1_act=new MoveAbsAction(des,this);
    cali1_act->t_end=2000;
    act_que.push_back(cali1_act);

    des[0]=0.5;
    des[1]=-0.3;
    des[2]=0;
    cali1_act=new MoveAbsAction(des,this);
    cali1_act->t_end=8000;
    act_que.push_back(cali1_act);

    cur_act=act_que.front();
    cur_act->initialize();
    act_que.pop_front();
    cout<<"start trajectory"<<endl;
    state=LOADTRACE;
}

// void CartesianController::genCalibTraj2()
// {
//
//   vector<double> des(3,0);
// //  int t=8000;
//   //move the the zero position.
// //   des[0]=0.35;
// //   des[1]=0;
// //   des[2]=0;
// //   act_que.push_back(new MoveAbsAction(des,this));
// //   act_que.back()->t_end=2000;
//
//   //double zz=0.03
//
//   des[0]=0.55;
//   des[1]=0;
//   des[2]=0;
//   cali1_act=new MoveAbsAction(des,this);
//   cali1_act->t_end=2000;
//   act_que.push_back(cali1_act);
//
//   des[0]=0.35;
//   des[1]=-0.05;
//   des[2]=0;
//   cali1_act=new MoveAbsAction(des,this);
//   cali1_act->t_end=8000;
//   act_que.push_back(cali1_act);
//
//   des[0]=0.55;
//   des[1]=-0.1;
//   des[2]=0;
//   cali1_act=new MoveAbsAction(des,this);
//   cali1_act->t_end=8000;
//   act_que.push_back(cali1_act);
//
//   des[0]=0.35;
//   des[1]=-0.15;
//   des[2]=0;
//   cali1_act=new MoveAbsAction(des,this);
//   cali1_act->t_end=8000;
//   act_que.push_back(cali1_act);
//
//   des[0]=0.55;
//   des[1]=-0.2;
//   des[2]=0;
//   cali1_act=new MoveAbsAction(des,this);
//   cali1_act->t_end=8000;
//   act_que.push_back(cali1_act);
//
//   des[0]=0.35;
//   des[1]=-0.2;
//   des[2]=0;
//   cali1_act=new MoveAbsAction(des,this);
//   cali1_act->t_end=8000;
//   act_que.push_back(cali1_act);
//
//   des[0]=0.55;
//   des[1]=-0.15;
//   des[2]=0;
//   cali1_act=new MoveAbsAction(des,this);
//   cali1_act->t_end=8000;
//   act_que.push_back(cali1_act);
//
//   des[0]=0.35;
//   des[1]=-0.1;
//   des[2]=0;
//   cali1_act=new MoveAbsAction(des,this);
//   cali1_act->t_end=8000;
//   act_que.push_back(cali1_act);
//
//   des[0]=0.55;
//   des[1]=-0.05;
//   des[2]=0;
//   cali1_act=new MoveAbsAction(des,this);
//   cali1_act->t_end=8000;
//   act_que.push_back(cali1_act);
//
//   des[0]=0.35;
//   des[1]=-0.0;
//   des[2]=0;
//   cali1_act=new MoveAbsAction(des,this);
//   cali1_act->t_end=8000;
//   act_que.push_back(cali1_act);
//
//
//   des[0]=0.55;
//   des[1]=-0.0;
//   des[2]=0;
//   cali2_act=new MoveAbsAction(des,this);
//   cali2_act->t_end=8000;
//   act_que.push_back(cali2_act);
//
//
//
//
//   cur_act=act_que.front();
//       cur_act->initialize();
//       act_que.pop_front();
//       cout<<"start trajectory"<<endl;
//   state=LOADTRACE;
// }

void CartesianController::calcLinkLength()
{

    int bin=calib1->bin;
    double* A[8];
    for (int i=0;i<8;i++)
        A[i]=new double[bin];
    double* b;
    b=new double[bin];

    for (int tt=0;tt<calib1->bin;tt++)
    {
        for (int i=0;i<8;i++)
            A[i][tt]=calib1->A[i][tt];
        b[tt]=calib1->b[tt];
    }

    Eigen::MatrixXf AA(8,8);
    for (int i=0;i<8;i++)
    {
        for (int j=0;j<8;j++)
        {
            double s=0;
            for (int k=0;k<bin;k++)
                s+=A[i][k]*A[j][k];
            AA(i,j)=s;
        }
    }
    cout<<AA<<endl;
    Eigen::VectorXf Ab(8);
    for (int i=0;i<8;i++)
    {
        double s=0;
        for (int j=0;j<bin;j++)
            s+=A[i][j]*b[j];
        Ab(i)=s;
    }
    cout<<Ab<<endl;
    Eigen::MatrixXf AV=AA.inverse();
    cout<<AV<<endl;
    Eigen::MatrixXf M=AV*Ab;
    //cout<<M<<endl;
//   for(int i=0;i<4;i++)
//   {
//     link[i]=Vector(M(i*2,0),M(i*2+1,0),0);
//   }
//   for(int i=0;i<4;i++)
//   {
//     for(int j=0;j<3;j++)
//       cout<<link[i][j]<<',';
//     cout<<endl;
//   }
}

void CartesianController::update()
{
    //n_cur++;
    rob->update();
    clock_gettime(CLOCK_REALTIME, &ts);
    //save the last joint position
//   for (int i=0;i<4;i++)
//     ql[i]=q[i];
    rob->motor2JointPosition(rob->q,q);
    rob->motor2JointVelocity(rob->v,qv);
    updateKinematics();
    for (int i=0;i<3;i++)
    {
        x[i]=frm0[5].p[i];
        v[i]=0;
        for (int j=0;j<4;j++)
            v[i]+=jacobian[j][i]*qv[j];
    }
    if (state==CALIBRATION)
    {
        //update desired position
        if (cur_act->finished)
        {
            if (cur_act->calib==NULL)
                delete cur_act;

            if (act_que.size()==0)
            {
                //calcLinkLength();
                state=SERVO;
                return;
            }
            cur_act=act_que.front();
            //cout<<cur_act->rel[3]<<endl;
            cur_act->initialize();
            act_que.pop_front();
        }
        cur_act->update();
        for (int i=0;i<3;i++)
        {
            xd[i]=cur_act->cmd.coordinates[i];
            vd[i]=cur_act->cmd.velocity[i];

        }
        //update torque command
        PDWithGC();

        logging();
    }
    else if (state==SEMIAUTO)
    {
        if (start_semi==true)
            cur_act->update2();
        if (end_semi==true)
        {
            state=SERVO;
            cur_act->calib->finish();
            calcLinkLength();
        }
        gravity_compensation();
        logging();
    }
    else if (state==LOADTRACE)
    {
        if (cur_act==NULL&&act_que.size()==0)
        {
            state=SERVO;
            return;
        }
        if (cur_act->finished)
        {
            if (cur_act->calib==NULL)
                delete cur_act;

            if (act_que.size()==0)
            {
                //calcGravityCompensation();
                state=SERVO;
                return;
            }

            cur_act=act_que.front();
            //cout<<cur_act->rel[3]<<endl;
            cur_act->initialize();
            act_que.pop_front();

        }
        cur_act->update();
        for (int i=0;i<3;i++)
        {
            xd[i]=cur_act->cmd.coordinates[i];
            vd[i]=cur_act->cmd.velocity[i];

        }
        //update torque command
        PDWithGC();
        //PDControl();
        logging();
    }
    else if (state==SERVO)
    {
        //PDControl();
        if (ctr_mode==PDWITHGC)
            PDWithGC();
        else if (ctr_mode==FULLSCAN)
            full_scan();
        else if (ctr_mode==PEAKFINDER)
          peak_finder();
            //PDControl();
        else if (ctr_mode==GRAVITY)
            gravity_compensation();
        else if (ctr_mode==NOCONTROL)
        {
            for (int i=0;i<4;i++)
                rob->torque[i]=tzero[i];
        }
        //floating();
        //gravity_compensation();
        logging();
    }
}

void CartesianController::startPDWithGC()
{
    ctr_mode=CartesianController::PDWITHGC;
    //space='m';
    for (int i=0;i<3;i++)
    {
        xd[i]=x[i];
        v[i]=0;
        s[i]=0;
        vd[i]=0;
    }
    cout<<"PD control(GC) in Cartesian space"<<endl;
}

void CartesianController::start_peak_finder()
{
    ctr_mode=CartesianController::PEAKFINDER;
    //space='m';
    for (int i=0;i<3;i++)
    {
        xd[i]=x[i];
        dx[i]=0;
        //dv[i]=0;
        v[i]=0;
        s[i]=0;
        vd[i]=0;
    }
    n_sum=0;
    n_cur=0;
    cnta=0;
    inductance_sum_last=0;
    search_mode=0;
    //for circular sweeping
    double phase=2.0*M_PI*(n_cur%cycle)/cycle;
    for (int i=0;i<3;i++)
        for (int j=0;j<3;j++)
            AA(i,j)=0;
    for (int i=0;i<3;i++)
        Ab(i)=0;

    center_x=x[0]-cos(phase)*radius;
    center_y=x[1]-sin(phase)*radius;
    cout<<"start sweeping the surface."<<endl;
}

void CartesianController::start_fullscan()
{
    ctr_mode=CartesianController::FULLSCAN;
    //space='m';
    for (int i=0;i<3;i++)
    {
        xd[i]=x[i];
        dx[i]=0;
        //dv[i]=0;
        v[i]=0;
        s[i]=0;
        vd[i]=0;
    }
    n_cur=0;

    cout<<"start sweeping the surface."<<endl;
}

void CartesianController::full_scan()
{
    n_cur++;
    updateKinematics();
    double width=0.25;
    double interval=0.01;
    double speed=0.05*1e-3;
    int nw=width/speed;
    int ni=interval/speed;
    int n_all=2*(nw+ni);
    int phase=n_cur%n_all;
    double eh=sqrt(dx[0]*dx[0]+dx[1]*dx[1]);
    if(last_inductance>2)
    {
        int freq = 2000; // freq in hz
  int len = 1; // len in ms

  //int fd = open("/dev/console", O_WRONLY);
  ioctl(fd, KIOCSOUND, (int)(1193180/freq));
  usleep(len);
  ioctl(fd, KIOCSOUND, 0);
  //close(fd);
    }
    //Ks[2]+=(eh-Ks[1])*fabs(eh-Ks[1])*Ks[0];
    if (phase<nw)
    {
        xd[0]-=speed;
    }
    else if (phase<nw+ni)
    {
        xd[1]-=speed;
    }
    else if (phase<nw+ni+nw)
    {
        xd[0]+=speed;
    }
    else
    {
        xd[1]-=speed;
    }

    xd[2]+=(eh-Ks[1])*fabs(eh-Ks[1])*Ks[0];
    //upper limit to avoid instability.
    if(xd[2]>Ks[2])
      xd[2]=Ks[2];
    //xd[2]=Ks[2]+eh*Ks[1];
    //if(xd[2]>Ks[2])
    //xd[2]=Ks[2];

    for (int i=0;i<3;i++)
        vd[i]=0;

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

        dx[i]=xd[i]-x[i];
        f[i]=Kp[i]*dx[i]-Kv[i]*(v[i]-vd[i]);
    }

    for (int i=0;i<4;i++)
    {
        tq[i]=0;
        for (int j=0;j<3;j++)
        {
            tq[i]+=jacobian[i][j]*f[j];
        }

    }

    //Gravity compensation
    //T=sum(gcj) dot (g*Zi)
    Vector g(0,0,-1);
    for (int i=0;i<4;i++)
    {
        Vector Zi=frm0[i+1].M.UnitZ();
        for (int j=i;j<4;j++)
        {
            Rotation R0j=frm0[j+1].M.Inverse();
            //coordinates of vector g*Zi in frame j
            Vector tmp=R0j*(g*Zi);
            tq[i]-=dot(gc[j],tmp);
        }
    }

    //resolve redundancy: away from joint limits.
    Eigen::Vector4f c;
    for (int i=0;i<4;i++)
        c(i)=-q[i];
    Eigen::Vector4f cf=nullspace*c;
    for (int i=0;i<4;i++)
        tq[i]+=Kc[i]*cf(i);

    //project force into motor space
    for (int i=0;i<4;i++)
    {
        tm[i]=0;
        for (int j=0;j<4;j++)
        {
            tm[i]+=rob->m2j[4*j+i]*tq[j];

        }

    }
    for (int i=0;i<4;i++)
        rob->torque[i]=tm[i];
}

/* This function sweeps the table surface in a zigzag manner
b=Ax
Eigen::MatrixXf AA(8,8);
Eigen::VectorXf Ab(8);
AAij=sum(xi*xj)
Ab=sum(xi*bi)
*/
void CartesianController::peak_finder()
{
    updateKinematics();
    n_cur++;
//  int cycle=int(mu);
//   double z_des=Ks[2];
//   double x_force=Ks[0];
//   double y_force=Ks[1];
//   //use position control in z direction
//   dx[2]=z_des-x[2];
//   f[2]=Kp[2]*dx[2]-Kv[2]*v[2];
//
//   f[0]=(n_cur%cycle>cycle/2)?x_force:-x_force;
//   f[1]=-y_force;

//   double vel_x=Ks[0]*1e-3;
//   double vel_y=Ks[1]*1e-3;
//   //double pos_z=Ks[2];
//   if(n_cur%cycle>=cycle/2)
//     vd[0]=vel_x;
//   else
//     vd[0]=-vel_x;
//   vd[1]=-vel_y;
//   vd[2]=0;
//   for(int i=0;i<2;i++)
//     xd[i]+=vd[i];
//     if(last_inductance>30)
//     {
//         int freq = 2000; // freq in hz
//   int len = 1; // len in ms
//
//   int fd = open("/dev/console", O_WRONLY);
//   ioctl(fd, KIOCSOUND, (int)(1193180/freq));
//   usleep(len);
//   ioctl(fd, KIOCSOUND, 0);
//   close(fd);
//     }

    double phase=2.0*M_PI*(n_cur%cycle)/cycle;
    double eh=sqrt(dx[0]*dx[0]+dx[1]*dx[1]);
    //Ks[2]+=(eh-0.03)/2;
    center_x+=dcx/cycle;
    center_y+=dcy/cycle;
    xd[0]=center_x+cos(phase)*radius;
    xd[1]=center_y+sin(phase)*radius;
    //xd[2]=Ks[2];
    xd[2]+=(eh-Ks[1])*fabs(eh-Ks[1])*Ks[0];
    //upper limit to avoid instability.
    if(xd[2]>Ks[2])
      xd[2]=Ks[2];
    int num_pts=20;
    int sample_cycle=cycle/num_pts;
    if (n_cur%sample_cycle==0)
    {
        AA(0,0)+=x[0]*x[0];
        //cout<<AA(0,0)<<endl;
        AA(0,1)+=x[0]*x[1];
        AA(0,2)+=x[0];
        AA(1,0)+=x[1]*x[0];
        AA(1,1)+=x[1]*x[1];
        AA(1,2)+=x[1];
        AA(2,0)+=x[0];
        AA(2,1)+=x[1];
        AA(2,2)+=1;
        Ab(0)+=x[0]*last_inductance;
        Ab(1)+=x[1]*last_inductance;
        Ab(2)+=last_inductance;
        n_sum++;
        //cout<<last_inductance<<',';
    }

    if (n_sum==num_pts)//has finished a circle
    {
       // cout<<endl;
        //cout<<AA<<endl<<endl;
        //cout<<Ab<<endl<<endl;
        //Eigen::Matrix3f inv=AA.inverse();
        //double det=AA.determinant();
        //cout<<"determinant:"<<AA.determinant()<<endl;
        //if (det>0.0025)

//         if (search_mode==0)
//         {
            if (Ab(2)>=num_pts)
            {

                if (Ab(2)>inductance_sum_last)
                {

                    M=AA.inverse()*Ab;
                    //cout<<M<<endl;
                    dcx=M(0,0)*6e-5;
                    dcy=M(1,0)*6e-5;
                    if (dcx>0.02)
                        dcx=0.02;
                    else if(dcx<-0.02)
                      dcx=-0.02;
                    if (dcy>0.02)
                        dcy=0.02;
                    else if(dcy<-0.02)
                      dcy=-0.02;
                    //center_x+=dcx;
                    //center_y+=dcy;
                    inductance_sum_last=Ab(2);
                    cnta=0;
                    //cout<<"update center(gradient):"<<center_x<<","<<center_y<<". inductance sum:"<<Ab(2)<<endl;
                    cout<<"inductance sum:"<<Ab(2)<<". direction:"<<dcx<<","<<dcy<<endl;
                }
                else
                {
//                     cnta++;
//                     if (cnta>3)
//                     {
//                         dcx=center_x-x[0];
//                         dcy=center_y-x[1];
//                         search_mode=1;
//                         cout<<"moving to the center"<<endl;
//                     }
//                     else
//                     {
//                       //cout<<"wait"<<". inductance sum:"<<Ab(2)<<endl;
                      dcx=0;
                      dcy=0;
                      cout<<"inductance sum:"<<Ab(2)<<". direction:"<<dcx<<","<<dcy<<endl;
//                    }
                }

            }
            else if (Ab(2)<num_pts)
            {
                //center_x+=0e-5;
                //center_y+=-1e-2;
                dcx=0;
                dcy=-0.01;
                //cout<<"update center(-y):"<<center_x<<","<<center_y<<". inductance sum:"<<Ab(2)<<endl;
                cout<<"inductance sum:"<<Ab(2)<<". direction:"<<dcx<<","<<dcy<<endl;
            }
       // }

        for (int i=0;i<3;i++)
                for (int j=0;j<3;j++)
                    AA(i,j)=0;
            for (int i=0;i<3;i++)
                Ab(i)=0;
        n_sum=0;
    }

    for (int i=0;i<3;i++)
        vd[i]=0;

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

        dx[i]=xd[i]-x[i];
        f[i]=Kp[i]*dx[i]-Kv[i]*(v[i]-vd[i]);
    }

    for (int i=0;i<4;i++)
    {
        tq[i]=0;
        for (int j=0;j<3;j++)
        {
            tq[i]+=jacobian[i][j]*f[j];
        }

    }

    //Gravity compensation
    //T=sum(gcj) dot (g*Zi)
    Vector g(0,0,-1);
    for (int i=0;i<4;i++)
    {
        Vector Zi=frm0[i+1].M.UnitZ();
        for (int j=i;j<4;j++)
        {
            Rotation R0j=frm0[j+1].M.Inverse();
            //coordinates of vector g*Zi in frame j
            Vector tmp=R0j*(g*Zi);
            tq[i]-=dot(gc[j],tmp);
        }
    }

    //resolve redundancy: away from joint limits.
    Eigen::Vector4f c;
    for (int i=0;i<4;i++)
        c(i)=-q[i];
    Eigen::Vector4f cf=nullspace*c;
    for (int i=0;i<4;i++)
        tq[i]+=Kc[i]*cf(i);

    //project force into motor space
    for (int i=0;i<4;i++)
    {
        tm[i]=0;
        for (int j=0;j<4;j++)
        {
            tm[i]+=rob->m2j[4*j+i]*tq[j];

        }

    }
    for (int i=0;i<4;i++)
        rob->torque[i]=tm[i];
}

void CartesianController::gravity_compensation()
{

    updateKinematics();
    //T=sum(gcj) dot (g*Zi
    Vector g(0,0,-1);
    //double tq[4];
    for (int i=0;i<4;i++)
    {
        Vector Zi=frm0[i+1].M.UnitZ();
        tq[i]=0;
        for (int j=i;j<4;j++)
        {
            Rotation R0j=frm0[j+1].M.Inverse();
            Vector tmp=R0j*(g*Zi);
            tq[i]-=dot(gc[j],tmp);
        }
    }

    for (int i=0;i<4;i++)
    {
        tm[i]=0;
        for (int j=0;j<4;j++)
        {
            tm[i]+=rob->m2j[4*j+i]*tq[j];

        }

    }
    for (int i=0;i<4;i++)
        rob->torque[i]=tm[i];
    //logging();

}

void CartesianController::PDWithGC()
{
    updateKinematics();
    //PID control in cartesian space
    for (int i=0;i<3;i++)
    {

        dx[i]=xd[i]-x[i];
        f[i]=Kp[i]*dx[i]-Kv[i]*(v[i]-vd[i]);
        s[i]=s[i]+dx[i];
        f[i]+=Ks[i]*s[i];
    }
    //for basket tip:modulating the normal force according to the horizontal error.
//   double eh=sqrt(dx[0]*dx[0]+dx[1]*dx[1]);
//   f[2]-=Kp[2]*eh*mu;
    //project force into joint space
    for (int i=0;i<4;i++)
    {
        tq[i]=0;
        for (int j=0;j<3;j++)
        {
            tq[i]+=jacobian[i][j]*f[j];
            //cout<<jacobian[4*j+i]<<',';
        }
        // cout<<endl;
    }

    //Gravity compensation
    //T=sum(gcj) dot (g*Zi)
    Vector g(0,0,-1);
    for (int i=0;i<4;i++)
    {
        Vector Zi=frm0[i+1].M.UnitZ();
        for (int j=i;j<4;j++)
        {
            Rotation R0j=frm0[j+1].M.Inverse();
            //coordinates of vector g*Zi in frame j
            Vector tmp=R0j*(g*Zi);
            tq[i]-=dot(gc[j],tmp);
        }
    }

    //resolve redundancy: away from joint limits.
    Eigen::Vector4f c;
    for (int i=0;i<4;i++)
        c(i)=-q[i];
    Eigen::Vector4f cf=nullspace*c;
    for (int i=0;i<4;i++)
        tq[i]+=Kc[i]*cf(i);

    //project force into motor space
    for (int i=0;i<4;i++)
    {
        tm[i]=0;
        for (int j=0;j<4;j++)
        {
            tm[i]+=rob->m2j[4*j+i]*tq[j];

        }

    }
    for (int i=0;i<4;i++)
        rob->torque[i]=tm[i];
    //logging();
}

void CartesianController::PDControl()
{
    for (int i=0;i<3;i++)
    {

        dx[i]=xd[i]-x[i];
        f[i]=Kp[i]*dx[i]-Kv[i]*(v[i]-vd[i]);
        s[i]=s[i]+dx[i];
        f[i]+=Ks[i]*s[i];
    }
    for (int i=0;i<4;i++)
    {
        tq[i]=0;
        for (int j=0;j<3;j++)
        {
            tq[i]+=jacobian[i][j]*f[j];
            //cout<<jacobian[4*j+i]<<',';
        }
        // cout<<endl;
    }
    for (int i=0;i<4;i++)
    {
        tm[i]=0;
        for (int j=0;j<4;j++)
        {
            tm[i]+=rob->m2j[4*j+i]*tq[j];

        }

    }
    for (int i=0;i<4;i++)
        rob->torque[i]=tm[i];
    //logging();
}

void CartesianController::set_position_ref()
{
    for (int i=0;i<4;i++)
    {
        mzero[i]=rob->q[i];
        qzero[i]=q[i];
    }
    for (int i=0;i<3;i++)
        xzero[i]=x[i];
}
void CartesianController::set_gc()
{
    for (int i=0;i<4;i++)
    {
        tzero[3]=tm[i];
    }
}
void CartesianController::clear_ref()
{
    for (int i=0;i<4;i++)
    {
        mzero[i]=0;
        qzero[i]=0;
        tzero[i]=0;
    }
    for (int i=0;i<3;i++)
        xzero[i]=0;
}

void CartesianController::logging()
{
    std::string log;
    std::stringstream out;
    for (int i=0;i<3;i++)
        out<<x[i]<<",";
    for (int i=0;i<3;i++)
        out<<xd[i]<<",";
    for (int i=0;i<3;i++)
        out<<v[i]<<",";
    for (int i=0;i<3;i++)
        out<<f[i]<<",";
    for (int i=0;i<3;i++)
        out<<(x[i]-xd[i])<<",";
    for (int i=0;i<4;i++)
        out<<q[i]<<',';
    for (int i=0;i<4;i++)
        out<<tq[i]<<',';
    for (int i=0;i<4;i++)
        out<<rob->q[i]<<',';
    for (int i=0;i<4;i++)
        out<<rob->v[i]<<',';
    for (int i=0;i<4;i++)
        out<<rob->torque[i]<<',';
    out << ts.tv_sec << ','<< ts.tv_nsec;
    log = out.str();
    diag.data=log;
    diag.header.stamp.sec=ts.tv_sec;
    diag.header.stamp.nsec=ts.tv_nsec;
    diagnostic_pub.publish(diag);
    //vector<double> cur_pos_act(q,q+4);
    js.header.stamp.sec=ts.tv_sec;
    js.header.stamp.nsec=ts.tv_nsec;
    //js.position=cur_pos_act;

    for (int i=0;i<3;i++)
    {
        js.position[i]=x[i];
        js.velocity[i]=v[i];
        js.effort[i]=f[i];
    }
    joint_state_pub.publish(js);
}
void CartesianController::stop()
{
  close(fd);
    rob->close();

}


/*
 * Copyright (C) 2017 koolkdev
 *
 * This software may be modified and distributed under the terms
 * of the MIT license.  See the LICENSE file for details.
 */

#include "DeviceEncryption.h"
#include <boost/endian/buffers.hpp>
#include <cryptopp/modes.h>
#include <cryptopp/aes.h>
#include <cryptopp/sha.h>

#include "Device.h"
struct WfsBlockIV {
	boost::endian::big_uint32_buf_t iv[4];
};

DeviceEncryption::DeviceEncryption(const std::shared_ptr<Device>& device, const std::vector<uint8_t>& key) : device(device), key(key)  {
}

void DeviceEncryption::HashData(const std::vector<uint8_t>& data, const std::vector<uint8_t>::iterator& hash) {
	// Pad and hash
	CryptoPP::SHA1 sha;
	sha.Update(&*data.begin(), data.size());
	std::vector<uint8_t> pad(ToSectorSize(data.size()) - data.size(), 0);
	if (!pad.empty())
		sha.Update(&*pad.begin(), pad.size());
	sha.Final(&*hash);
}

void DeviceEncryption::CalculateHash(const std::vector<uint8_t>& data, const std::vector<uint8_t>::iterator& hash, bool hash_in_block) {
	// Fill hash space with 0xFF
	if (hash_in_block)
		std::fill(hash, hash + DIGEST_SIZE, 0xFF);

	HashData(data, hash);
}

void DeviceEncryption::WriteBlock(uint32_t sector_address, const std::vector<uint8_t>& data, uint32_t iv, bool encrypt) {
	// Pad with zeros
	std::vector<uint8_t> enc_data(data);
	enc_data.resize(ToSectorSize(data.size()), 0);

	uint32_t sectors_count = static_cast<uint32_t>(enc_data.size() / this->device->GetSectorSize());

	if (encrypt) {
		// Encrypt
		auto _iv = GetIV(sectors_count, iv);
		CryptoPP::CBC_Mode<CryptoPP::AES>::Encryption encryptor(&*key.begin(), key.size(), reinterpret_cast<uint8_t *>(&_iv));
		encryptor.ProcessData(&*enc_data.begin(), &*enc_data.begin(), enc_data.size());
	}

	// Write
	this->device->WriteSectors(enc_data, sector_address, sectors_count);
}

std::vector<uint8_t> DeviceEncryption::ReadBlock(uint32_t sector_address, uint32_t length, uint32_t iv, bool encrypt) {
	uint8_t sectors_count = static_cast<uint8_t>(ToSectorSize(length) / this->device->GetSectorSize());

	std::vector<uint8_t> data = this->device->ReadSectors(sector_address, sectors_count);

	if (encrypt) {
		auto _iv = GetIV(sectors_count, iv);
		CryptoPP::CBC_Mode<CryptoPP::AES>::Decryption decryptor(&*key.begin(), key.size(), reinterpret_cast<uint8_t *>(&_iv));
		decryptor.ProcessData(&*data.begin(), &*data.begin(), data.size());
	}

	//data.resize(length);
	return data;
}

bool DeviceEncryption::CheckHash(const std::vector<uint8_t>& data, const std::vector<uint8_t>::iterator& hash, bool hash_in_block) {
	std::vector<uint8_t> placeholder_hash(DIGEST_SIZE, 0xFF);
	if (hash_in_block)
		std::swap_ranges(placeholder_hash.begin(), placeholder_hash.end(), hash);

	std::vector<uint8_t> calculated_hash(DIGEST_SIZE);
	HashData(data, calculated_hash.begin());

	 if (hash_in_block)
		 std::swap_ranges(placeholder_hash.begin(), placeholder_hash.end(), hash);

	 return std::equal(calculated_hash.begin(), calculated_hash.end(), hash);
}

WfsBlockIV DeviceEncryption::GetIV(uint32_t sectors_count, uint32_t iv) {
	WfsBlockIV aes_iv;
	aes_iv.iv[0] = sectors_count * this->device->GetSectorSize();
	aes_iv.iv[1] = iv;
	aes_iv.iv[2] = this->device->GetSectorsCount();
	aes_iv.iv[3] = this->device->GetSectorSize();
	return aes_iv;
}

size_t DeviceEncryption::ToSectorSize(size_t size) {
	return size + (-static_cast<int32_t>(size)) % this->device->GetSectorSize();
}


#include <stdio.h>
#include <iostream>

#include <dlib/dnn.h>
#include <dlib/data_io.h>
#include <dlib/image_processing.h>

#include <boost/thread.hpp>
#include "boost/filesystem/operations.hpp"
#include "boost/filesystem/path.hpp"
#include "boost/progress.hpp"

namespace fs = boost::filesystem;

using namespace dlib;

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

template <long num_filters, typename SUBNET> using con5d = con<num_filters,5,5,2,2,SUBNET>;
template <long num_filters, typename SUBNET> using con5  = con<num_filters,5,5,1,1,SUBNET>;

template <typename SUBNET> using downsampler  = relu<affine<con5d<32, relu<affine<con5d<32, relu<affine<con5d<16,SUBNET>>>>>>>>>;
template <typename SUBNET> using rcon5  = relu<affine<con5<45,SUBNET>>>;

using net_type = loss_mmod<con<1,9,9,1,1,rcon5<rcon5<rcon5<downsampler<input_rgb_image_pyramid<pyramid_down<6>>>>>>>>;

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

void face_location(size_t thread_id, net_type* net, matrix<rgb_pixel>* mat)
{
    std::cout << "start thread_id: " << thread_id << std::endl;
    try
    {
        auto& n = *net;
        auto& m = *mat;
        auto dets = n(m);
        for (auto&& d : dets)
            std::cout << d.rect << std::endl;
    }
    catch (...)
    {
        std::cout << "catch exception!!" << std::endl;
    }
    std::cout << "finish thread_id: " << thread_id << std::endl;
}

int main(int argc, char* argv[])
{
    net_type net;
    deserialize(argv[1]) >> net;
    std::cout << "deserialize: " << argv[1] << std::endl;
    const size_t thread_num = atoi(argv[2]);
    std::cout << "thread_num: " << thread_num << std::endl;

    matrix<rgb_pixel> img;
    load_image(img, argv[3]);
    std::cout << "load_image: " << argv[3] << std::endl;

    while(img.size() < 1800*1800)
        pyramid_up(img);

    // prepare thread
    std::vector<boost::thread*> threads(thread_num);
    for (size_t i = 0; i < thread_num; i++)
        threads[i] = new boost::thread(boost::bind(face_location, i, &net, &img));
    std::cout << "prepare thread" << std::endl;

    // wait thread
    std::cout << "wait all thread" << std::endl;
    for (size_t i = 0; i < thread_num; i++)
        threads[i]->join();
    std::cout << "finish all thread" << std::endl;

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_SWAR_FUNCTIONS_SIMD_SSE_AVX_ARITH_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_SWAR_FUNCTIONS_SIMD_SSE_AVX_ARITH_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_AVX_SUPPORT

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

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_
                                    , boost::simd::tag::avx_
                                    , (T)
                                    , ((target_ < simd_ < double_<T>
                                                        , boost::simd::tag::avx_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(T const&) const
    {
      return _mm256_set_pd(3.0,2.0,1.0,0.0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_
                                    , boost::simd::tag::avx_
                                    , (T)
                                    , ((target_ < simd_ <  single_<T>
                                                        , boost::simd::tag::avx_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(T const&) const
    {
      return _mm256_set_ps(7.0f,6.0f,5.0f,4.0f,3.0f,2.0f,1.0f,0.0f);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_
                                    , boost::simd::tag::avx_
                                    , (T)
                                    , ((target_ < simd_ < ints64_<T>
                                                        , boost::simd::tag::avx_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(T const&) const
    {
      return bitwise_cast<result_type>(_mm256_set_epi32(0,3,0,2,0,1,0,0));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_
                                    , boost::simd::tag::avx_
                                    , (T)
                                    , ((target_ < simd_ < ints32_<T>
                                                        , boost::simd::tag::avx_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(T const&) const
    {
      return _mm256_set_epi32(7,6,5,4,3,2,1,0);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_
                                    , boost::simd::tag::avx_
                                    , (T)
                                    , ((target_ < simd_ < ints16_<T>
                                                        , boost::simd::tag::avx_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(T const&) const
    {
      return _mm256_set_epi16( 15, 14, 13, 12, 11, 10, 9, 8
                                           , 7,  6,  5,  4,  3,  2,  1, 0
                                           );
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::arith_
                                    , boost::simd::tag::avx_
                                    , (T)
                                    , ((target_ < simd_ < ints8_<T>
                                                        , boost::simd::tag::avx_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename T::type result_type;

    result_type operator()(T const&) const
    {
      return _mm256_set_epi8( 31, 30, 29, 28, 27, 26, 25, 24
                                          , 23, 22, 21, 20, 19, 18, 17, 16
                                          , 15, 14, 13, 12, 11, 10, 9,  8
                                          , 7,  6,  5,  4,  3,  2,  1,  0
                                          );
    }
  };

} } }

#endif
#endif


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2017.
//
// This software is released under a three-clause BSD license:
//  * 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 any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// 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 ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS 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.
//
// --------------------------------------------------------------------------
// $Maintainer: Timo Sachsenberg $
// $Authors: Clemens Groepl, Chris Bielow $
// --------------------------------------------------------------------------

#include <boost/math/distributions/cauchy.hpp>
#include <OpenMS/TRANSFORMATIONS/FEATUREFINDER/IsotopeModel.h>
#include <OpenMS/MATH/STATISTICS/BasicStatistics.h>
#include <OpenMS/CHEMISTRY/EmpiricalFormula.h>

namespace OpenMS
{
  IsotopeModel::IsotopeModel() :
    InterpolationModel(),
    charge_(0),
    monoisotopic_mz_(0.0)
  {
    setName(getProductName());

    defaults_.setValue("averagines:C", 0.04443989f, "Number of C atoms per Dalton of mass.", ListUtils::create<String>("advanced"));
    defaults_.setValue("averagines:H", 0.06981572f, "Number of H atoms per Dalton of mass.", ListUtils::create<String>("advanced"));
    defaults_.setValue("averagines:N", 0.01221773f, "Number of N atoms per Dalton of mass.", ListUtils::create<String>("advanced"));
    defaults_.setValue("averagines:O", 0.01329399f, "Number of O atoms per Dalton of mass.", ListUtils::create<String>("advanced"));
    defaults_.setValue("averagines:S", 0.00037525f, "Number of S atoms per Dalton of mass.", ListUtils::create<String>("advanced"));
    defaults_.setValue("isotope:trim_right_cutoff", 0.001, "Cutoff in averagine distribution, trailing isotopes below this relative intensity are not considered.", ListUtils::create<String>("advanced"));
    defaults_.setValue("isotope:maximum", 100, "Maximum isotopic rank to be considered.", ListUtils::create<String>("advanced"));
    defaults_.setValue("isotope:distance", 1.000495, "Distance between consecutive isotopic peaks.", ListUtils::create<String>("advanced"));

    defaults_.setValue("isotope:mode:mode", "Gaussian", "Peak Shape used around each isotope peak.", ListUtils::create<String>("advanced"));
    defaults_.setValidStrings("isotope:mode:mode", ListUtils::create<String>("Gaussian,Lorentzian"));
    defaults_.setValue("isotope:mode:LorentzFWHM", 0.3, "Full width of the Lorentzian (Cauchy) function applied to the averagine isotopic pattern to simulate the inaccuracy of the mass spectrometer.", ListUtils::create<String>("advanced"));
    defaults_.setValue("isotope:mode:GaussianSD", 0.1, "Standard deviation of Gaussian applied to the averagine isotopic pattern to simulate the inaccuracy of the mass spectrometer.", ListUtils::create<String>("advanced"));

    defaults_.setValue("charge", 1, "Charge state of the model.", ListUtils::create<String>("advanced"));
    defaults_.setValue("statistics:mean", 0.0, "Centroid m/z (as opposed to monoisotopic m/z).", ListUtils::create<String>("advanced"));

    defaultsToParam_();
  }

  IsotopeModel::IsotopeModel(const IsotopeModel & source) :
    InterpolationModel(source)
  {
    setParameters(source.getParameters());
    updateMembers_();
  }

  IsotopeModel::~IsotopeModel()
  {
  }

  IsotopeModel & IsotopeModel::operator=(const IsotopeModel & source)
  {
    if (&source == this)
      return *this;

    InterpolationModel::operator=(source);
    setParameters(source.getParameters());
    updateMembers_();

    return *this;
  }

  EmpiricalFormula IsotopeModel::getFormula()
  {
    CoordinateType mass = mean_ * charge_;

    Int C_num = Int(0.5 + mass * averagine_[C]);
    Int N_num = Int(0.5 + mass * averagine_[N]);
    Int O_num = Int(0.5 + mass * averagine_[O]);
    Int H_num = Int(0.5 + mass * averagine_[H]);
    Int S_num = Int(0.5 + mass * averagine_[S]);

    String form;
    if (C_num)
      form.append("C").append(String(C_num));
    if (H_num)
      form.append("H").append(String(H_num));
    if (N_num)
      form.append("N").append(String(N_num));
    if (O_num)
      form.append("O").append(String(O_num));
    if (S_num)
      form.append("S").append(String(S_num));

    return EmpiricalFormula(form);
  }

  const IsotopeDistribution & IsotopeModel::getIsotopeDistribution() const
  {
    return isotope_distribution_;
  }

  void IsotopeModel::setSamples(const EmpiricalFormula & formula)
  {
    typedef std::vector<double> ContainerType;
    ContainerType isotopes_exact;

    isotope_distribution_ = formula.getIsotopeDistribution(max_isotope_);

    isotope_distribution_.trimRight(trim_right_cutoff_);
    isotope_distribution_.renormalize();

    // compute the average mass (-offset)
    CoordinateType isotopes_mean = 0;
    {
      Int cnt = 0;
      for (IsotopeDistribution::iterator iter = isotope_distribution_.begin();
           iter != isotope_distribution_.end(); ++iter, ++cnt)
      {
        isotopes_exact.push_back(iter->second);
        isotopes_mean += iter->second * cnt;
      }
      isotopes_mean *= isotope_distance_ / charge_;
    }
    // (Need not divide by sum of probabilities, which is 1.)

    ///
    // "stretch" the averagine isotope distribution (so we can add datapoints between isotope peaks)
    ///
    size_t isotopes_exact_size = isotopes_exact.size();
    isotopes_exact.resize(size_t((isotopes_exact_size - 1) * isotope_distance_ / interpolation_step_ + 1.6)); // round up a bit more

    for (Size i = isotopes_exact_size - 1; i; --i)
    {
      // we don't need to move the 0-th entry
      isotopes_exact[size_t(CoordinateType(i) * isotope_distance_ / interpolation_step_ / charge_ + 0.5)]
        =   isotopes_exact[i];
      isotopes_exact[i] = 0;
    }

    ////
    // compute the Gaussian/Cauchy distribution (to be added for widening the averagine isotope distribution)
    ////
    ContainerType peak_shape_values_y;
    // fill a container with CoordinateType points (x values)
    CoordinateType peak_width = 0.0;
    if (param_.getValue("isotope:mode:mode") == "Gaussian")
    {
      // Actual width for values in the smooth table for normal distribution
      peak_width = isotope_stdev_ * 4.0;  // MAGIC alert, num stdev for smooth table for normal distribution
      ContainerType peak_shape_values_x;
      for (double coord = -peak_width; coord <= peak_width;
           coord += interpolation_step_)
      {
        peak_shape_values_x.push_back(coord);
      }
      // compute normal approximation at these CoordinateType points (y values)
      Math::BasicStatistics<> normal_widening_model;
      normal_widening_model.setSum(1);
      normal_widening_model.setMean(0);
      normal_widening_model.setVariance(isotope_stdev_ * isotope_stdev_);
      normal_widening_model.normalApproximation(peak_shape_values_y, peak_shape_values_x);
    }
    else if (param_.getValue("isotope:mode:mode") == "Lorentzian")
    {
      peak_width = isotope_lorentz_fwhm_ * 8.0; // MAGIC alert: Lorentzian has infinite support, but we need to stop sampling at some point: 8*FWHM
      for (double coord = -peak_width; coord <= peak_width;
           coord += interpolation_step_)
      {
        boost::math::cauchy_distribution<double> cauchy(0., isotope_lorentz_fwhm_ / 2.0);
        double x = boost::math::pdf(cauchy, coord);
        peak_shape_values_y.push_back(x); //cauchy is using HWHM not FWHM
      }
    }

    ///
    // fold the Gaussian/Lorentzian at each averagine peak, i.e. fill linear interpolation
    ///
    const ContainerType & left = isotopes_exact;
    const ContainerType & right = peak_shape_values_y;
    ContainerType & result = interpolation_.getData();
    result.clear();

    SignedSize r_max = std::min(SignedSize(left.size() + right.size() - 1),
                                SignedSize(2 * peak_width / interpolation_step_ * max_isotope_ + 1));
    result.resize(r_max, 0);

    // we loop backwards because then the small products tend to come first
    // (for better numerics)
    for (SignedSize i = left.size() - 1; i >= 0; --i)
    {
      if (left[i] == 0)
        continue;
      for (SignedSize j = std::min(r_max - i, SignedSize(right.size())) - 1; j >= 0; --j)
      {
        result[i + j] += left[i] * right[j];
      }
    }

    monoisotopic_mz_ = mean_ - isotopes_mean;
    interpolation_.setMapping(interpolation_step_, peak_width / interpolation_step_, monoisotopic_mz_);

    //std::cerr << "mono now: " << monoisotopic_mz_ << " mono easy: " << formula.getMonoWeight()/formula.getCharge() << "\n";

    // scale data so that integral over distribution equals one
    // multiply sum by interpolation_step_ -> rectangular approximation of integral
    IntensityType factor = scaling_ / (interpolation_step_ * std::accumulate(result.begin(), result.end(), IntensityType(0)));
    for (ContainerType::iterator iter = result.begin(); iter != result.end(); ++iter)
    {
      *iter *= factor;
    }
  }

  void IsotopeModel::setOffset(CoordinateType offset)
  {
    double diff = offset - getInterpolation().getOffset();
    mean_ += diff;
    monoisotopic_mz_ += diff;

    InterpolationModel::setOffset(offset);

    param_.setValue("statistics:mean", mean_);
  }

  IsotopeModel::CoordinateType IsotopeModel::getOffset()
  {
    return getInterpolation().getOffset();
  }

  UInt IsotopeModel::getCharge()
  {
    return charge_;
  }

  IsotopeModel::CoordinateType IsotopeModel::getCenter() const
  {
    return monoisotopic_mz_;
  }

  void IsotopeModel::updateMembers_()
  {
    InterpolationModel::updateMembers_();

    charge_ = param_.getValue("charge");
    isotope_stdev_ = param_.getValue("isotope:mode:GaussianSD");
    isotope_lorentz_fwhm_ = param_.getValue("isotope:mode:LorentzFWHM");
    mean_ = param_.getValue("statistics:mean");
    max_isotope_ = param_.getValue("isotope:maximum");
    trim_right_cutoff_ = param_.getValue("isotope:trim_right_cutoff");
    isotope_distance_ = param_.getValue("isotope:distance");

    averagine_[C] = param_.getValue("averagines:C");
    averagine_[H] = param_.getValue("averagines:H");
    averagine_[N] = param_.getValue("averagines:N");
    averagine_[O] = param_.getValue("averagines:O");
    averagine_[S] = param_.getValue("averagines:S");

  }

}


//==============================================================================
//         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
//==============================================================================
#ifndef NT2_REDUCTION_FUNCTIONS_CONTAINER_MAXIMUM_HPP_INCLUDED
#define NT2_REDUCTION_FUNCTIONS_CONTAINER_MAXIMUM_HPP_INCLUDED

#include <nt2/reduction/functions/maximum.hpp>
#include <nt2/core/container/dsl/reduction.hpp>
#include <boost/simd/reduction/functions/maximum.hpp>

#include <nt2/include/functions/max.hpp>
#include <nt2/include/constants/maxinit.hpp>

namespace nt2 { namespace ext
{
  template<class Domain, class Expr>
  struct  size_of<boost::simd::tag::maximum_,Domain,1,Expr>
        : meta::reduction_size<Expr,1>
  {};

  template<class Domain, class Expr>
  struct  size_of<boost::simd::tag::maximum_,Domain,2,Expr>
        : meta::reduction_size<Expr,2>
  {};

  template<class Domain, int N, class Expr>
  struct  value_type<boost::simd::tag::maximum_,Domain,N,Expr>
        : meta::reduction_value<boost::simd::tag::maximum_,Expr>
  {};
} }

#endif


#ifndef COSMO_PROFILES
#define COSMO_PROFILES

#include "COSMO_SAC/util.hpp"
#include <Eigen/Dense>
#include <map>
#include <limits>
#include <iterator>
#include <algorithm>
#include <cctype>
#include <functional>
#include "nlohmann/json.hpp"

namespace COSMOSAC {

/**
A single sigma profile.  In this data structure (for consistency with the 2005 Virginia Tech
database of COSMO-SAC parameters), the first column is the electron density in [e/A^2], and the second column
is the probability of finding a segment with this electron density, multiplied by the segment area, in A^2
*/
class SigmaProfile {
public:
    Eigen::ArrayXd m_sigma, m_psigmaA;
    /// Default constructor
    SigmaProfile() {};
    /// Copy-by-reference constructor
    SigmaProfile(const Eigen::ArrayXd &sigma, const Eigen::ArrayXd &psigmaA) : m_sigma(sigma), m_psigmaA(psigmaA) {};
    /// Move constructor
    SigmaProfile(Eigen::ArrayXd &&sigma, Eigen::ArrayXd &&psigmaA) : m_sigma(sigma), m_psigmaA(psigmaA) {};
    const Eigen::ArrayXd &psigmaA() const { return m_psigmaA; }
    const Eigen::ArrayXd &sigma() const { return m_sigma; }
    const Eigen::ArrayXd psigma(double A_i) const { return m_psigmaA/A_i; }

};

struct SigmaProfileSet {
    SigmaProfile nhb, ///< The profile for the non-hydrogen-bonding segments
        oh, ///< The profile for the OH-bonding segments
        ot; ///< The profile for the "other" segments
};

/// The sigma profiles (and some more metadata) associated with the fluid
struct FluidProfiles {
    enum class dispersion_classes{DISP_WATER, DISP_COOH, DISP_NHB, DISP_ONLY_ACCEPTOR, DISP_DONOR_ACCEPTOR};
    SigmaProfileSet profiles;
    std::string name;
    std::size_t VTnumber;
    double A_COSMO_A2; ///< The surface area of the molecule as calculated by COSMO-SAC, in A^2
    double V_COSMO_A3; ///< The volume of the molecule as calculated by COSMO-SAC, in A^3
    dispersion_classes dispersion_flag;
    double dispersion_eoverkB;
    nlohmann::json meta; ///< Any additional metadata, stored in JSON format
};

///
class ProfileDatabase {
private:
    std::map<std::string, FluidProfiles> datadb;
public:
    FluidProfiles get_profile(const std::string & identifier) const{
        if (datadb.find(identifier) != datadb.cend()) {
            return datadb.find(identifier)->second;
        }
        else {
            throw std::invalid_argument("Unable to load the profile for the identifier [" + identifier + "]");
        }
    }
    virtual std::string normalize_identifier(const std::string &identifier) const { return identifier; };
    std::size_t length(const std::string & identifier) {
        return datadb.size();
    }
    void clear() {
        datadb.clear();
    }
    void add_to_db(const std::string & identifier, FluidProfiles &&value) {
        datadb[identifier] = value;
    }
};

struct VTFluidInfo {
    int VTIndex;
    std::string numstring;
    std::string name;
    std::string CAS;
    double V_COSMO_A3;
};
void to_json(nlohmann::json& j, const VTFluidInfo& p) {
    j = nlohmann::json{{"VTIndex", p.VTIndex}, {"numstring", p.numstring}, {"name", p.name}, {"CAS", p.CAS},  {"V_COSMO_A3", p.V_COSMO_A3}};
}
void from_json(const nlohmann::json& j, VTFluidInfo& p) {
    j.at("VTIndex").get_to(p.VTIndex);
    j.at("numstring").get_to(p.numstring);
    j.at("name").get_to(p.name);
    j.at("CAS").get_to(p.CAS);
    j.at("V_COSMO_A3").get_to(p.V_COSMO_A3);
}

/// The format of the VirginiaTech database of profiles
class VirginiaTechProfileDatabase : public ProfileDatabase{
private:

    std::map<std::string, VTFluidInfo> m_VTdata;
    const std::string m_dbpath;
public:
    VirginiaTechProfileDatabase(const std::string &tab_delimited_summary, const std::string &dbpath) : m_dbpath(dbpath) {
        // First, read in the file of summary information
        auto lines = str_split(get_file_contents(tab_delimited_summary), "\n");
        if (lines.size() == 1){
            lines = str_split(get_file_contents(tab_delimited_summary), "\r");
        }
        if (lines.size() == 1){
            throw std::invalid_argument("Unable to match the line endings in file " + tab_delimited_summary);
        }
        // Iterate over the lines, storing an entry for each one
        for (auto &&line : lines) {
            VTFluidInfo fluid;
            if (line.empty()) { continue; }
            auto elements = str_split(strstrip(line), "\t");
            if (elements.size() < 6) {
                throw std::invalid_argument("Need at least 6 entries in line");
            }
            if (elements[0] == "Index No.") {
                continue;
            }
            fluid.VTIndex = mystrtoi(elements[0]);
            // Index 1 is a chemical formula, but non-standard form
            fluid.name = elements[2];
            fluid.CAS = elements[3];
            fluid.V_COSMO_A3 = mystrtod(elements[5]);
            char num[5];
            snprintf(num, sizeof(num), "%04d", static_cast<int>(fluid.VTIndex));
            fluid.numstring = num;
            if (m_VTdata.find(fluid.numstring) != m_VTdata.end()) {
                throw std::invalid_argument("Cannot have duplicate key: "+ fluid.numstring);
            }
            else{
                m_VTdata[fluid.numstring] = fluid;
            }

        }
    }
    std::string normalize_identifier(const std::string &identifier) const override {
        // The key is in the database, return it
        if (m_VTdata.find(identifier) != m_VTdata.end()) {
            return identifier;
        }
        for (auto &el : m_VTdata) {
            auto &fluid = el.second;
            if (fluid.name == identifier)
            {
                // Construct the fluid identifier from the identifier that is provided
                char num[5];
                snprintf(num, sizeof(num), "%04d", static_cast<int>(fluid.VTIndex));
                return num;
            }
            if (fluid.CAS == identifier)
            {
                // Construct the fluid identifier from the identifier that is provided
                char num[5];
                snprintf(num, sizeof(num), "%04d", static_cast<int>(fluid.VTIndex));
                return num;
            }
        }
        throw std::invalid_argument("Unable to match identifier in secondary lookup");
    }
    void add_profile(std::string identifier) {
        auto info = m_VTdata[identifier];
        // Now we load the sigma profile(s) from the file
        auto lines = str_split(get_file_contents(m_dbpath + "/VT2005-" + info.numstring + "-PROF.txt"));
        std::vector<double> sigma, psigmaA;
        FluidProfiles fluid;
        for (auto &&line : lines) {
            if (line.size() > 10) {
                auto _sigma = mystrtod(line.substr(0, 25));
                auto _psigmaA = mystrtod(line.substr(25, 25));
                sigma.push_back(_sigma);
                psigmaA.push_back(_psigmaA);
            }
        }
        fluid.name = info.name;
        fluid.VTnumber = info.VTIndex;
        fluid.V_COSMO_A3 = info.V_COSMO_A3;
        if (sigma.size() == 51 && psigmaA.size() == 51) {
            fluid.profiles.nhb = SigmaProfile(Eigen::Map<Eigen::ArrayXd>(&(sigma[0]), sigma.size()),
                Eigen::Map<Eigen::ArrayXd>(&(psigmaA[0]), psigmaA.size()));
            fluid.A_COSMO_A2 = fluid.profiles.nhb.psigmaA().sum();
        }
        else {
            throw std::invalid_argument("Don't support 2 & 3 sigma profiles yet");
        }
        add_to_db(info.numstring, std::move(fluid));
    }
    std::string to_JSON(){
        auto nspaces = 2;
        nlohmann::json j = m_VTdata;
        return j.dump(nspaces);
    }
};

struct DelawareFluidInfo {
    std::size_t DelIndex;
    std::string CAS;
    std::string formula;
    std::string name;
    std::string SMILES;
    std::string InChIString; ///< Standard InChI string
    std::string InChIKey; ///< Standard InChI key
    double V_COSMO_A3;
    nlohmann::json meta;
};
void to_json(nlohmann::json& j, const DelawareFluidInfo& p) {
    j = nlohmann::json{
        {"DelIndex", p.DelIndex}, {"CAS", p.CAS}, {"formula", p.formula},  {"name", p.name},
        {"SMILES", p.SMILES}, {"InChIString", p.InChIString}, {"InChIKey", p.InChIKey},  {"V_COSMO_A3", p.V_COSMO_A3}
};
}
void from_json(const nlohmann::json& j, DelawareFluidInfo& p) {
    j.at("DelIndex").get_to(p.DelIndex);
    j.at("CAS").get_to(p.CAS);
    j.at("formula").get_to(p.formula);
    j.at("name").get_to(p.name);
    j.at("SMILES").get_to(p.SMILES);
    j.at("InChIString").get_to(p.InChIString);
    j.at("InChIKey").get_to(p.InChIKey);
    j.at("V_COSMO_A3").get_to(p.V_COSMO_A3);
    j.at("name").get_to(p.name);
}

/// The format of the Delaware database of profiles
class DelawareProfileDatabase : public ProfileDatabase {
private:

    std::map<std::string, DelawareFluidInfo> m_Deldata;
    const std::string m_dbpath;
public:
    DelawareProfileDatabase(const std::string &space_delimited_summary, const std::string &dbpath) : m_dbpath(dbpath) {
        // First, read in the file of summary information
        auto lines = str_split(get_file_contents(space_delimited_summary), "\n");
        // Iterate over the lines, storing an entry for each one
        for (auto &&line : lines) {
            DelawareFluidInfo fluid;
            if (line.empty()) { continue; }
            auto elements = str_split(strrstrip(line), " ");
            if (elements[0] == "ID") {
                continue;
            }
            fluid.DelIndex = mystrtoi(elements[0]);
            fluid.formula = elements[1];
            fluid.CAS = elements[2];
            fluid.name = elements[3];
            fluid.SMILES = elements[4];
            fluid.InChIString = elements[5];
            fluid.InChIKey = elements[6];
            m_Deldata[fluid.InChIKey] = fluid;
        }
    }
    std::string normalize_identifier(const std::string &identifier) const override {
        // The key is in the database, return it
        if (m_Deldata.find(identifier) != m_Deldata.end()) {
            return identifier;
        }
        for (auto &el : m_Deldata) {
            auto &fluid = el.second;
            if (fluid.name == identifier)
            {
                return fluid.InChIKey;
            }
            if (fluid.CAS == identifier)
            {
                return fluid.InChIKey;
            }
        }
        throw std::invalid_argument("Unable to match identifier in secondary lookup");
    }
    void add_profile(std::string identifier) {
        auto info = m_Deldata[identifier];
        // Now we load the sigma profile(s) from the file
        auto lines = str_split(get_file_contents(m_dbpath + "/" + info.InChIKey + ".sigma"));

        std::vector<double> _sigma, _psigmaA;
        FluidProfiles fluid;
        std::string meta;
        for (auto &&line : lines) {
            if (line.substr(0,8) == "# Name: "){ std::string check_name = line.substr(8,line.size()-8); continue;}
            if (line.substr(0,8) == "# CASn: ") { std::string check_CAS = line.substr(8, line.size() - 8); continue; }
            if (line.substr(0, 8) == "# meta: ") { meta = line.substr(8, line.size() - 8); continue; }
            if (line[0] == '#'){ continue; }
            auto v = str_split(strrstrip(line), " ");
            if (v.empty() || (v.size() ==1 && v[0].empty())){ continue; }
            _sigma.push_back(mystrtod(v[0]));
            _psigmaA.push_back(mystrtod(v[1]));
        }
        Eigen::Map<Eigen::ArrayXd> sigma(&(_sigma[0]), _sigma.size());
        Eigen::Map<Eigen::ArrayXd> psigmaA(&(_psigmaA[0]), _psigmaA.size());
        fluid.name = info.name;

        fluid.VTnumber = info.DelIndex;
        if (sigma.size() == 51 && psigmaA.size() == 51) {
            fluid.profiles.nhb = SigmaProfile(sigma, psigmaA);
            fluid.A_COSMO_A2 = fluid.profiles.nhb.psigmaA().sum();
        }
        else if (sigma.size() == 51*3 && psigmaA.size() == 51*3){
            fluid.profiles.nhb = SigmaProfile(sigma.segment(0*51, 51), psigmaA.segment(0*51, 51));
            fluid.profiles.oh =  SigmaProfile(sigma.segment(1*51, 51), psigmaA.segment(1*51, 51));
            fluid.profiles.ot =  SigmaProfile(sigma.segment(2*51, 51), psigmaA.segment(2*51, 51));
            double check_Area2 = fluid.profiles.nhb.psigmaA().sum() + fluid.profiles.oh.psigmaA().sum() + fluid.profiles.ot.psigmaA().sum();
            fluid.A_COSMO_A2 = check_Area2;
        }
        else{
            throw std::invalid_argument("Length of sigma profile ["+std::to_string(sigma.size())+"] is neither 51 nor 51*3");
        }
        fluid.meta = nlohmann::json::parse(meta);
        fluid.V_COSMO_A3 = fluid.meta["volume [A^3]"];
        std::string flag = fluid.meta["disp. flag"];
        if (flag == "COOH") {
            fluid.dispersion_flag = FluidProfiles::dispersion_classes::DISP_COOH;
        }
        else if (flag == "H2O") {
            fluid.dispersion_flag = FluidProfiles::dispersion_classes::DISP_WATER;
        }
        else if (flag == "NHB") {
            fluid.dispersion_flag = FluidProfiles::dispersion_classes::DISP_NHB;
        }
        else if (flag == "HB-ACCEPTOR") {
            fluid.dispersion_flag = FluidProfiles::dispersion_classes::DISP_ONLY_ACCEPTOR;
        }
        else if (flag == "HB-DONOR-ACCEPTOR") {
            fluid.dispersion_flag = FluidProfiles::dispersion_classes::DISP_DONOR_ACCEPTOR;
        }
        else {
            throw std::invalid_argument("Unable to match dispersion flag: \""+flag+"\"");
        }
        if (fluid.meta["disp. e/kB [K]"].is_null()){
            // The null from JSON is mapped to a NaN of C++
            fluid.dispersion_eoverkB = std::numeric_limits<double>::quiet_NaN();
        }
        else {
            fluid.dispersion_eoverkB = fluid.meta["disp. e/kB [K]"];
        }
        add_to_db(info.InChIKey, std::move(fluid));
    }
    std::string to_JSON(){
        auto nspaces = 2;
        nlohmann::json j = m_Deldata;
        return j.dump(nspaces);
    }
};

} /* namespace COSMOSAC */

#endif


#include "dynet/nodes.h"
#include "dynet/dynet.h"
#include "dynet/training.h"
#include "dynet/timing.h"
#include "dynet/rnn.h"
#include "dynet/gru.h"
#include "dynet/lstm.h"
#include "dynet/fast-lstm.h"
#include "dynet/expr.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <type_traits>
#include <sys/types.h>
#include <sys/stat.h>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/program_options.hpp>

#include "s2s/dynet/dict.h"
#include "s2s/corpus/options.hpp"
#include "s2s/corpus/corpora.hpp"
#include "s2s/corpus/dicts.hpp"
#include "s2s/corpus/batch.hpp"
#include "s2s/nn/encdec.hpp"

#ifndef INCLUDE_GUARD_TAGGER_HPP
#define INCLUDE_GUARD_TAGGER_HPP

namespace s2s {

class tagger : public encoder_decoder {

public:

    std::vector<dynet::Expression> h_bi;

    explicit tagger(dynet::ParameterCollection& model, const s2s_options& opts, dicts& d) : encoder_decoder(model,opts,d) {

        flag_drop_out = true;

        if(opts.additional_output_layer == true){
            p_add_W = model.add_parameters({opts.rnn_size, opts.rnn_size * 2});
            p_add_bias = model.add_parameters({opts.rnn_size});
            p_out_R = model.add_parameters({d.dict_set_trg.d_word.size(), opts.rnn_size});
            p_out_bias = model.add_parameters({d.dict_set_trg.d_word.size()});
        }else{
            p_out_R = model.add_parameters({d.dict_set_trg.d_word.size(), opts.rnn_size * 2});
            p_out_bias = model.add_parameters({d.dict_set_trg.d_word.size()});
        }

    }

    tagger(const tagger&) = delete;
    tagger& operator=(const tagger&) = delete;
    ~tagger() = default;

    void encoder(const batch &batch_local, dynet::ComputationGraph& cg) {
        // initialize
        init(batch_local, cg);
        // embedding
        embedding(batch_local, cg);

        std::vector<dynet::Expression> h_fwd(slen);
        std::vector<dynet::Expression> h_bwd(slen);
        h_bi.resize(slen);

        // forward encoder
        fwd_enc_builder.new_graph(cg);
        fwd_enc_builder.start_new_sequence();
        if(flag_drop_out == true && opts.dropout_rate_lstm_word > 0.f){
            fwd_enc_builder.set_dropout_masks(batch_size);
        }
        for (unsigned int t_i = 0; t_i < slen; ++t_i) {
            fwd_enc_builder.add_input(h_lookup[t_i]);
            h_fwd[t_i] = fwd_enc_builder.back();
        }
        // backward encoder
        rev_enc_builder.new_graph(cg);
        rev_enc_builder.start_new_sequence();
        if(flag_drop_out == true && opts.dropout_rate_lstm_word > 0.f){
            rev_enc_builder.set_dropout_masks(batch_size);
        }
        for (unsigned int ind = 0; ind < slen; ++ind) {
            unsigned int t_i = (slen - 1) - ind;
            rev_enc_builder.add_input(h_lookup[t_i]);
            h_bwd[t_i] = rev_enc_builder.back();
        }
        // bidirectional encoding
        for (unsigned int t_i = 0; t_i < slen; ++t_i) {
            h_bi[t_i] = concatenate(std::vector<dynet::Expression>({h_fwd[t_i], h_bwd[t_i]}));
        }
    }

    dynet::Expression output(dynet::ComputationGraph& cg, const unsigned int t){
        dynet::Expression i_final;
        if(opts.additional_output_layer){
            dynet::Expression i_add_W = parameter(cg, p_add_W);
            dynet::Expression i_add_bias = parameter(cg, p_add_bias);
            i_final = tanh(i_add_W * h_bi[t] + i_add_bias);
        }else{
            i_final = h_bi[t];
        }
        dynet::Expression i_out_R = parameter(cg, p_out_R);
        dynet::Expression i_out_bias = parameter(cg, p_out_bias);
        dynet::Expression i_out_pred_t = i_out_R * i_final + i_out_bias;
        return i_out_pred_t;
    }

    void disable_dropout(){
        fwd_char_enc_builder.disable_dropout();
        rev_char_enc_builder.disable_dropout();
        fwd_enc_builder.disable_dropout();
        rev_enc_builder.disable_dropout();
        flag_drop_out = false;
    }

    void enable_dropout(){
        fwd_char_enc_builder.set_dropout(opts.dropout_rate_lstm_char, 0.f);
        rev_char_enc_builder.set_dropout(opts.dropout_rate_lstm_char, 0.f);
        fwd_enc_builder.set_dropout(opts.dropout_rate_lstm_word, 0.f);
        rev_enc_builder.set_dropout(opts.dropout_rate_lstm_word, 0.f);
        flag_drop_out = true;
    }

private:
    friend class boost::serialization::access;
    template<class Archive>
    void serialize(Archive & ar, const unsigned int version) {
        ar & p_word_enc;
        ar & p_char_enc;
        ar & p_feat_enc;
        ar & p_layer_w;
        ar & p_add_W;
        ar & p_add_bias;
        ar & p_out_R;
        ar & p_out_bias;
        ar & rev_char_enc_builder;
        ar & fwd_char_enc_builder;
        ar & rev_enc_builder;
        ar & fwd_enc_builder;
    }

};

};

#endif


/*
 Copyright (C) 2017 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*
  Copyright (C) 2014 Jose Aparicio
  Copyright (C) 2014 Peter Caspers

  This file is part of QuantLib, a free-software/open-source library
  for financial quantitative analysts and developers - http://quantlib.org/

  QuantLib is free software: you can redistribute it and/or modify it
  under the terms of the QuantLib license.  You should have received a
  copy of the license along with this program; if not, please email
  <quantlib-dev@lists.sf.net>. The license is also available online at
  <http://quantlib.org/license.shtml>.

  This program is distributed in the hope that it will be useful, but WITHOUT
  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
  FOR A PARTICULAR PURPOSE.  See the license for more details.
 */

#include <ql/time/calendars/weekendsonly.hpp>
#include <ql/time/daycounters/actual360.hpp>
#include <qle/instruments/makecds.hpp>

#include <boost/make_shared.hpp>

namespace QuantExt {

MakeCreditDefaultSwap::MakeCreditDefaultSwap(const Period& tenor, const Real couponRate)
    : side_(Protection::Buyer), nominal_(1.0), tenor_(tenor), termDate_(boost::none), couponTenor_(3 * Months),
      couponRate_(couponRate), upfrontRate_(0.0),
      dayCounter_(Actual360()), lastPeriodDayCounter_(Actual360(true)),
      rule_(DateGeneration::CDS2015), cashSettlementDays_(3) {}

MakeCreditDefaultSwap::MakeCreditDefaultSwap(const Date& termDate, const Real couponRate)
    : side_(Protection::Buyer), nominal_(1.0), tenor_(boost::none), termDate_(termDate), couponTenor_(3 * Months),
      couponRate_(couponRate), upfrontRate_(0.0),
      dayCounter_(Actual360()), lastPeriodDayCounter_(Actual360(true)),
      rule_(DateGeneration::CDS2015), cashSettlementDays_(3) {}

MakeCreditDefaultSwap::operator CreditDefaultSwap() const {
    boost::shared_ptr<CreditDefaultSwap> swap = *this;
    return *swap;
}

MakeCreditDefaultSwap::operator boost::shared_ptr<CreditDefaultSwap>() const {

    Date tradeDate = Settings::instance().evaluationDate();
    Date upfrontDate = WeekendsOnly().advance(tradeDate, cashSettlementDays_, Days);

    Date protectionStart;
    if (rule_ == DateGeneration::CDS2015 || rule_ == DateGeneration::CDS) {
        protectionStart = tradeDate;
    } else {
        protectionStart = tradeDate + 1;
    }

    Date end;
    if (tenor_) {
        if (rule_ == DateGeneration::CDS2015 || rule_ == DateGeneration::CDS || rule_ == DateGeneration::OldCDS) {
            end = cdsMaturity(tradeDate, *tenor_, rule_);
        } else {
            end = tradeDate + *tenor_;
        }
    } else {
        end = *termDate_;
    }

    Schedule schedule(protectionStart, end, couponTenor_, WeekendsOnly(), Following, Unadjusted, rule_, false);

    boost::shared_ptr<CreditDefaultSwap> cds = boost::make_shared<CreditDefaultSwap>(
        side_, nominal_, upfrontRate_, couponRate_, schedule, Following, dayCounter_, true,
        CreditDefaultSwap::atDefault, protectionStart, upfrontDate, boost::shared_ptr<Claim>(),
        lastPeriodDayCounter_, tradeDate, cashSettlementDays_);

    cds->setPricingEngine(engine_);
    return cds;
}

MakeCreditDefaultSwap& MakeCreditDefaultSwap::withUpfrontRate(Real upfrontRate) {
    upfrontRate_ = upfrontRate;
    return *this;
}

MakeCreditDefaultSwap& MakeCreditDefaultSwap::withSide(Protection::Side side) {
    side_ = side;
    return *this;
}

MakeCreditDefaultSwap& MakeCreditDefaultSwap::withNominal(Real nominal) {
    nominal_ = nominal;
    return *this;
}

MakeCreditDefaultSwap& MakeCreditDefaultSwap::withCouponTenor(Period couponTenor) {
    couponTenor_ = couponTenor;
    return *this;
}

MakeCreditDefaultSwap& MakeCreditDefaultSwap::withDayCounter(DayCounter& dayCounter) {
    dayCounter_ = dayCounter;
    return *this;
}

MakeCreditDefaultSwap& MakeCreditDefaultSwap::withLastPeriodDayCounter(DayCounter& lastPeriodDayCounter) {
    lastPeriodDayCounter_ = lastPeriodDayCounter;
    return *this;
}

MakeCreditDefaultSwap& MakeCreditDefaultSwap::withDateGenerationRule(DateGeneration::Rule rule) {
    rule_ = rule;
    return *this;
}

MakeCreditDefaultSwap& MakeCreditDefaultSwap::withCashSettlementDays(Natural cashSettlementDays) {
    cashSettlementDays_ = cashSettlementDays;
    return *this;
}

MakeCreditDefaultSwap& MakeCreditDefaultSwap::withPricingEngine(const boost::shared_ptr<PricingEngine>& engine) {
    engine_ = engine;
    return *this;
}
} // namespace QuantExt


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

#include "chainparams.h"
#include "validation.h"

#include "test/test_dash.h"

#include <boost/test/unit_test.hpp>

BOOST_FIXTURE_TEST_SUITE(subsidy_tests, TestingSetup)

BOOST_AUTO_TEST_CASE(block_subsidy_test)
{
    const Consensus::Params& consensusParams = Params(CBaseChainParams::MAIN).GetConsensus();

    uint32_t nPrevBits;
    int32_t nPrevHeight;
    CAmount nSubsidy;

    nPrevBits = 0x1c4a47c4;
    nPrevHeight = 4249;
    nSubsidy = GetBlockSubsidy(nPrevBits, nPrevHeight, consensusParams, false);
    BOOST_CHECK_EQUAL(nSubsidy, 157500000000ULL);

    // 1st subsidy reduction happens here

    // details for block 210240 (subsidy returned will be for block 210241)
    nPrevBits = 0x1b10d50b;
    nPrevHeight = 210240;
    nSubsidy = GetBlockSubsidy(nPrevBits, nPrevHeight, consensusParams, false);
    BOOST_CHECK_EQUAL(nSubsidy, 143325000000ULL);
}

BOOST_AUTO_TEST_SUITE_END()


/*!
 * @name      CerberusAnymalControlBase.hpp
 * @author    Samuel Zimmermann
 * @email     zsamuel@ethz.ch
 * @date      April 30, 2020
 *
 * Copyright (C) 2020 Robotic Systems Lab, ETH Zurich.
 * All rights reserved.
 * http://www.rsl.ethz.ch/
 */

#pragma once

// C++
#include <memory>

// Boost
#include <boost/thread.hpp>

// Eigen
#include <Eigen/Core>

// Learning Controller
#include "cerberus_anymal_utils/GraphLoader.hpp"

namespace cerberus_anymal_control {

//!@brief Fixed variable number of joints since controller only works with 12 joints.
static constexpr unsigned int numberOfJoints = 12;

using GenCoordinates = Eigen::Matrix<double, 19, 1>;
using GenVelocities = Eigen::Matrix<double, 18, 1>;
using Twist = Eigen::Matrix<double, 6, 1>;

template <typename ConcreteQuadrupedController>
class CerberusAnymalControlBase {
 public:
  CerberusAnymalControlBase();
  ~CerberusAnymalControlBase() = default;

 protected:
  virtual void initialize() = 0;
  virtual void loadParameters(const std::string& path);
  void advance();

  //! @brief Pointer to the specific learning controller
  std::unique_ptr<ConcreteQuadrupedController> quadrupedController_;

  //! @brief Graph for the learning controller
  GraphLoader<float> graph_;

 protected:
  //! @brief Coordinates for the controller
  //! Position(x,y,z),Orientation(w,x,y,z),JointPositions(LF_HAA,LF_HFE,LF_KFE,RF_HAA,RF_HFE,RF_KFE,LH_HAA,LH_HFE,LH_KFE,RH_HAA,RH_HFE,RH_KFE)
  GenCoordinates genCoordinates_;
  //! LinearVelocity(x,y,z),AngularVelocity(x,y,z),JointVelocities(LF_HAA,LF_HFE,LF_KFE,RF_HAA,RF_HFE,RF_KFE,LH_HAA,LH_HFE,LH_KFE,RH_HAA,RH_HFE,RH_KFE)
  GenVelocities genVelocities_;
  //! LinearVelocity(x,y,z), AngularVelocity(x,y,z)
  Twist desTwist_;
  Eigen::VectorXd desJointPositions_;

  //! @brief Mutex for the controller data
  mutable boost::shared_mutex data_mutex_;

  //! @brief Checks that data has arrived
  bool hasBodyVelocityData_;
  bool hasJointPositionData_;
  bool initializedGraph_;
};

} // namespace cerberus_anymal_control

#include "CerberusAnymalControlBase.tpp"


// Copyright 2017 Thomas Schöps
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//
// 2. 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.
//
// 3. Neither the name of the copyright holder 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 HOLDER 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.
#ifndef IMAGES_HPP
#define IMAGES_HPP

#include <cstring>
#include <memory>
// #include <unordered_map>
#include <map>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/StdVector>

struct ColmapFeatureObservation {
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    // Sub-pixel coordinates of the observation in its image, given in pixels.
    Eigen::Vector2f xy;

    // Id of the corresponding 3D point or -1 if no 3D point is associated to this
    // observation.
    int point3d_id;
};

// Type for transformations in SE3. This example code sticks to Eigen, however
// the SE3f type from Sophus may be better suited, for example (since it uses
// quaternions for storing the rotation in contrast to the class chosen here,
// which uses a matrix for storage and allows all affine transformations for
// it in principle).
typedef Eigen::Transform<float, 3, Eigen::Affine> SE3f;

// Holds data of a COLMAP image.
struct ColmapImage {
    // Unique image id.
    int image_id;

    // Id of the camera model for this image.
    int camera_id;

    // Path to the image file, may be a relative path.
    std::string file_path;

    Eigen::Vector3f position;

    // Global-to-image transformation.
    SE3f image_T_global;

    // Image-to-global transformation.
    SE3f global_T_image;

    // 2D feature observations in this image.
    std::vector<ColmapFeatureObservation, Eigen::aligned_allocator<ColmapFeatureObservation>> observations;
};

typedef std::shared_ptr<ColmapImage> ColmapImagePtr;
typedef std::shared_ptr<const ColmapImage> ColmapImageConstPtr;

typedef std::vector<ColmapImagePtr> ColmapImagePtrVector;
// typedef std::unordered_map<int, ColmapImagePtr> ColmapImagePtrMap;
typedef std::map<int, ColmapImagePtr>
    ColmapImagePtrMap; // I change unordered_map to map so that we can process according to the image_id order

// Loads ColmapImagePtr from a COLMAP images.txt file and appends them
// to the images map (indexed by image_id). Returns true if successful.
bool ReadColmapImages(const std::string &images_txt_path, bool read_observations, ColmapImagePtrMap &images);

#endif


#include "onlinenode.h"
#include "utilities.h"
#include <boost/foreach.hpp>

int OnlineNode::m_numNodes;

OnlineNode::OnlineNode(const HyperParameters &hp, int depth, int numFeatures) : m_hp(&hp), m_depth( depth ),
        m_numSamples( 0 ), m_nodeLabel( 0 ), m_numFeatures( numFeatures ),
        m_bestGini( 1e11 ), m_secondBestGini( 1e11 )
{
    m_isLeaf = true;
    m_nodeIndex = m_numNodes++;
    m_nodeConf = std::vector<float>(m_hp->numClasses,0.0);
    m_labelStatistics = std::vector<int>(m_hp->numClasses,0);
}

OnlineNode::OnlineNode(const HyperParameters &hp, int depth, int numFeatures, std::vector<int>& labelStatistics) :
        m_hp(&hp), m_depth( depth ), m_numSamples( 0 ), m_labelStatistics( labelStatistics ),
        m_numFeatures( numFeatures ),m_bestGini( 1e11 ), m_secondBestGini( 1e11 )
{
    int maxLabel = 0;
    int i = 0;
    BOOST_FOREACH(int label, m_labelStatistics) {
        if (label > maxLabel) {
            m_nodeLabel = i;
            maxLabel = label;
        }
        ++i;
    }

    m_isLeaf = true;
    m_nodeIndex = m_numNodes++;
    m_nodeConf.reserve(m_hp->numClasses);
}

OnlineNode::~OnlineNode() {

}

bool OnlineNode::shouldISplit() {
    // Test depth and sample size
    double epsilon = sqrt( pow(log(m_hp->numClasses),2.0) * log(100.0) / (2.0 * double(m_numSamples)) );
    double deltaGini = abs(m_bestGini - m_secondBestGini);
    //m_epsilons.push_back(epsilon);
    //m_deltaGinis.push_back(deltaGini);
    if (m_depth >= m_hp->maxTreeDepth || m_numSamples < m_hp->counterThreshold || deltaGini > epsilon) {
        return false;
    }

    // Test pureness of the node
    bool isPure = false;
    BOOST_FOREACH(int s, m_labelStatistics) {
        if (s == m_numSamples) {
            isPure = true;
            break;
        }
    }
    if (isPure) {
        return false;
    } else {
        return true;
    }

    return true;
}

void OnlineNode::getLabels(std::set<int>& nodeLabels) {
    if (!m_isLeaf) {
        m_leftChildNode->getLabels(nodeLabels);
        m_rightChildNode->getLabels(nodeLabels);
    } else {
        nodeLabels.insert(m_nodeLabel);
    }

    return;
}


// Copyright 2015-2022 The ALMA Project Developers
//
// 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.

#pragma once

/// @file
/// Code related to the dynamical matrix.
#include <boost/serialization/complex.hpp>
#include <boost/serialization/array.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/unique_ptr.hpp>
#include <structures.hpp>
#include <symmetry.hpp>

namespace alma {
/// POD class that holds all the information about the harmonic
/// properties of the system at a particular q point.
/// The public members are not declared as const to make
/// serialization simpler, but as a general rule it is probably not
/// a good idea to change them.
class Spectrum_at_point {
private:
    friend class boost::serialization::access;
    /// Serialize the data needed to reconstruct an
    /// object of this class.
    ///
    /// @param[in,out] ar - an output archive
    /// @param[in] version - version number used
    /// internally by boost::serialization
    template <class Archive>
    void save(Archive& ar, const unsigned int version) const {
        auto rows = this->omega.size();

        ar << rows;
        ar << boost::serialization::make_array(this->omega.data(), rows);
        rows = this->wfs.rows();
        auto cols = this->wfs.cols();
        ar << rows;
        ar << cols;
        ar << boost::serialization::make_array(this->wfs.data(), rows * cols);
        rows = this->vg.rows();
        cols = this->vg.cols();
        ar << rows;
        ar << cols;
        ar << boost::serialization::make_array(this->vg.data(), rows * cols);
    }

    /// Unserialize the data needed to reconstruct an
    /// object of this class.
    ///
    /// @param[in,out] ar - an output archive
    /// @param[in] version - version number used
    /// internally by boost::serialization
    template <class Archive>
    void load(Archive& ar, const unsigned int version) {
        decltype(this->omega.size()) rows;
        ar >> rows;
        this->omega.resize(rows);
        ar >> boost::serialization::make_array(this->omega.data(), rows);
        decltype(rows) cols;
        ar >> rows;
        ar >> cols;
        this->wfs.resize(rows, cols);
        ar >> boost::serialization::make_array(this->wfs.data(), rows * cols);
        ar >> rows;
        ar >> cols;
        this->vg.resize(rows, cols);
        ar >> boost::serialization::make_array(this->vg.data(), rows * cols);
    }

    // Instruct boost::serialization to expect separate load()
    // and save() methods.
    BOOST_SERIALIZATION_SPLIT_MEMBER()
public:
    /// Angular frequencies, in rad / ps. Imaginary values are
    /// represented as negative real numbers.
    Eigen::ArrayXd omega;
    /// Eigenvectors, normalized on a single unit cell. Each
    /// column is an eigenvector.
    Eigen::MatrixXcd wfs;
    /// Cartesian components of the group velocities in km / s.
    Eigen::ArrayXXd vg;
    /// Basic constructor.
    Spectrum_at_point(const Eigen::Ref<const Eigen::ArrayXd>& _omega,
                      const Eigen::Ref<const Eigen::MatrixXcd>& _wfs,
                      const Eigen::Ref<const Eigen::ArrayXXd>& _vg)
        : omega(_omega), wfs(_wfs), vg(_vg) {
    }

    /// Empty default constructor.
    Spectrum_at_point() {
    }
};

/// Factory of Dynamical_matrix objects.
class Dynamical_matrix_builder {
public:
    /// Constructor for nonpolar systems.
    Dynamical_matrix_builder(const Crystal_structure& _structure,
                             const Symmetry_operations& syms,
                             const Harmonic_ifcs& fcs);
    /// Constructor for polar systems.
    Dynamical_matrix_builder(const Crystal_structure& _structure,
                             const Symmetry_operations& syms,
                             const Harmonic_ifcs& fcs,
                             const Dielectric_parameters& born);
    /// Return the dynamical matrix and its derivatives
    /// at one point.
    ///
    /// @param[in] q - the q point in Cartesian coordinates
    /// @return an array containing the dynamical matrix and the
    /// three components of its gradient
    std::array<Eigen::MatrixXcd, 4> build(
        const Eigen::Ref<const Eigen::Vector3d>& q) const;

    /// Call build() and use the return value to obtain the
    /// harmonic properties at one q point.
    ///
    /// @param[in] q - the q point in Cartesian coordinates
    /// @return a Spectrum_at_point object with all relevant
    /// information.
    std::unique_ptr<Spectrum_at_point> get_spectrum(
        const Eigen::Ref<const Eigen::Vector3d>& q) const;

    /// Obtain the number of building blocks of unfolded,
    /// mass-reduced interatomic force constants used to build the
    /// dynamical matrix.
    ///
    /// @return the number of such blocks
    std::size_t get_nblocks() const {
        return this->blocks.size();
    }
    /// Obtain a copy of one of the building blocks of the
    /// dynamical matrix.
    ///
    /// @param[in] i - block index
    /// @return a matrix containing a copy of the block
    Eigen::MatrixXd get_block(std::size_t i) const {
        if (i >= this->blocks.size())
            throw value_error("invalid index");
        return this->blocks[i];
    }

    /// Obtain the direct coordinates of the lattice point
    /// associated to a block.
    ///
    /// @param[in] i - block index
    /// @return three integers identifying a lattice point.
    Triple_int get_pos(std::size_t i) const {
        if (i >= this->blocks.size())
            throw value_error("invalid index");
        return this->pos[i];
    }

private:
    /// Building blocks of the dynamical matrix. Those are built
    /// from the IFC matrices and the masses, taking into account
    /// the degeneracy of each pair of atoms.
    std::vector<Eigen::MatrixXd> blocks;
    /// Matrices used to kee track of the degeneracy of atom pairs in order to
    /// apply the nonanalytic correction
    std::vector<Eigen::MatrixXd> masks;
    /// Lattice coordinates of each block.
    std::vector<Triple_int> pos;
    /// Dimension of the supercell originally used for the IFC
    /// calculations along the first axis.
    const int na;
    /// Dimension of the supercell originally used for the IFC
    /// calculations along the second axis.
    const int nb;
    /// Dimension of the supercell originally used for the IFC
    /// calculations along the third axis.
    const int nc;
    /// Volume of the unit cell.
    const double V;
    /// Description of the crystal.
    const Crystal_structure structure;
    /// Reciprocal-space lattice vectors of the structure.
    const Eigen::MatrixXd rlattvec;
    /// Object describing the space group symmetries.
    const Symmetry_operations symmetries;
    /// Lattice coordinates in an alternative format.
    Eigen::MatrixXd mpos;
    /// Cartesian coordinates of each block.
    Eigen::MatrixXd cpos;
    /// Square matrix with 3 * natoms rows where element ij is equal
    /// to the square root of the products of the masses of atom i/3
    /// and atom j/3.
    Eigen::ArrayXXd massmatrix;
    /// True if the Coulomb nonanalytic correction is taken into
    /// account.
    const bool nonanalytic;
    /// Dielectric parameters required for the nonanalytic
    /// correction.
    const Dielectric_parameters born;
    /// Populate the "blocks" member variable using the data
    /// provided to the constructor.
    ///
    /// @param[in] structure - a description of the unit cell
    /// @param[in] fcs - an object containing the IFCs
    /// for a supercell
    void copy_blocks(const Harmonic_ifcs& fcs);

    /// Compute the complex exponential weights needed to build the
    /// dynamical matrix out of the blocks vector.
    ///
    /// @param[in] q - the q point in Cartesian coordinates
    /// @return - a vector of weights
    Eigen::ArrayXcd get_exponentials(
        const Eigen::Ref<const Eigen::Vector3d>& q) const;

    /// Return the nonanalytic part of the dynamical matrix and
    /// its derivatives at one point.
    ///
    /// @param[in] q - the q point in Cartesian coordinates
    /// @return an array containing the long-range contribution to
    /// the force constants and the three components of
    /// its gradient.
    std::array<Eigen::ArrayXXd, 4> build_nac(
        const Eigen::Ref<const Eigen::Vector3d>& q) const;
};
} // namespace alma