#include <v3d_interface.h>
#include "v3d_message.h"
#include "tracing_func.h"
#include "../../../hackathon/zhi/APP2_large_scale/readRawfile_func.h"
#include "../../../released_plugins/v3d_plugins/istitch/y_imglib.h"
#include "../../../released_plugins/v3d_plugins/neurontracing_vn2/app2/my_surf_objs.h"
#include "../../../released_plugins/v3d_plugins/neurontracing_vn2/vn_app2.h"
#include "../../../released_plugins/v3d_plugins/neurontracing_vn2/vn_app1.h"
#include "swc_surf_objs.h"
#include "stack"
#include "neurontracer_plugin.h"
#include "../../../released_plugins/v3d_plugins/sort_neuron_swc/sort_swc.h"
#include "../../../released_plugins/v3d_plugins/istitch/y_imglib.h"
#include "../../../released_plugins/v3d_plugins/resample_swc/resampling.h"
#include "../../../released_plugins/v3d_plugins/neuron_image_profiling/profile_swc.h"
#include "../../../v3d_main/jba/c++/convert_type2uint8.h"
//#include "../../../hackathon/zhi/AllenNeuron_postprocessing/sort_swc_IVSCC.h"
#include "../../../released_plugins/v3d_plugins/mean_shift_center/mean_shift_fun.h"
#include "../../../released_plugins/v3d_plugins/neurontracing_vn2/app1/v3dneuron_gd_tracing.h"
#include "../../../hackathon/zhi/branch_point_detection/branch_pt_detection_func.h"
#include "../../../../v3d_external/v3d_main/basic_c_fun/stackutil.h"
#include "../anisodiffusion_LXF/src/q_imgaussian3D.h"
#include "../anisodiffusion_LXF/src/q_EigenVectors3D.h"
#include "../anisodiffusion_LXF/src/q_derivatives3D.h"
#include "../anisodiffusion_LXF/src/q_AnisoDiff3D.h"
#include "../soma_remove/soma_remove_main.h"
//#include "../anisodiffusion_LXF/src/EigenDecomposition3.h"
//#include "../anisodiffusion_LXF/src/EigenDecomposition3.h"
bool export_2dtif(V3DPluginCallback & cb,const char * filename, unsigned char * pdata, V3DLONG sz[3], int datatype);
extern int thresh;
extern NeuronTree trace_result,resultTree_rebase,resultTree;
LandmarkList marker_rebase,marker_rebase2;
LandmarkList marker_rebase3;  //original marker
LocationSimple simple_rebase,true_rebase,LXF;
extern LocationSimple next_m;
V3DLONG marker_num_rebase=0;
V3DLONG thres_rebase=42;
QString outimg_file;
extern bool change;
bool is_soma =false;
extern bool enhance;
#if  defined(Q_OS_LINUX)
    #include <omp.h>
#endif

template <class T>
void BinaryProcess(T *apsInput, T * aspOutput, V3DLONG iImageWidth, V3DLONG iImageHeight, V3DLONG iImageLayer, V3DLONG h, V3DLONG d)
{
    V3DLONG i, j,k,n,count;
    double t, temp;

    V3DLONG mCount = iImageHeight * iImageWidth;
    for (i=0; i<iImageLayer; i++)
    {
        for (j=0; j<iImageHeight; j++)
        {
            for (k=0; k<iImageWidth; k++)
            {
                V3DLONG curpos = i * mCount + j*iImageWidth + k;
                V3DLONG curpos1 = i* mCount + j*iImageWidth;
                V3DLONG curpos2 = j* iImageWidth + k;
                temp = 0;
                count = 0;
                for(n =1 ; n <= d  ;n++)
                {
                    if (k>h*n) {temp += apsInput[curpos1 + k-(h*n)]; count++;}
                    if (k+(h*n)< iImageWidth) { temp += apsInput[curpos1 + k+(h*n)]; count++;}
                    if (j>h*n) {temp += apsInput[i* mCount + (j-(h*n))*iImageWidth + k]; count++;}//
                    if (j+(h*n)<iImageHeight) {temp += apsInput[i* mCount + (j+(h*n))*iImageWidth + k]; count++;}//
                    if (i>(h*n)) {temp += apsInput[(i-(h*n))* mCount + curpos2]; count++;}//
                    if (i+(h*n)< iImageLayer) {temp += apsInput[(i+(h*n))* mCount + j* iImageWidth + k ]; count++;}
                }
                t =  apsInput[curpos]-temp/(count);
                aspOutput[curpos]= (t > 0)? t : 0;
            }
        }
    }
}

#include <boost/lexical_cast.hpp>
template <class T> T pow2(T a)
{
    return a*a;

}

QString getAppPath();

using namespace std;

#define getParent(n,nt) ((nt).listNeuron.at(n).pn<0)?(1000000000):((nt).hashNeuron.value((nt).listNeuron.at(n).pn))
bool export_list2file(vector<MyMarker*> & outmarkers, QString fileSaveName, QString fileOpenName)
{
    QFile file(fileSaveName);
    if (!file.open(QIODevice::WriteOnly|QIODevice::Text))
        return false;
    QTextStream myfile(&file);

    QFile qf(fileOpenName);
    if (! qf.open(QIODevice::ReadOnly | QIODevice::Text))
    {
        return false;
    }
    QString info;
    while (! qf.atEnd())
    {
        char _buf[1000], *buf;
        qf.readLine(_buf, sizeof(_buf));
        for (buf=_buf; (*buf && *buf==' '); buf++); //skip space

        if (buf[0]=='\0')	continue;
        if (buf[0]=='#')
        {
           info = buf;
           myfile<< info.remove('\n') <<endl;
        }

    }

    map<MyMarker*, int> ind;
    for(int i = 0; i < outmarkers.size(); i++) ind[outmarkers[i]] = i+1;

    for(V3DLONG i = 0; i < outmarkers.size(); i++)
    {
        MyMarker * marker = outmarkers[i];
        int parent_id;
        if(marker->parent == 0) parent_id = -1;
        else parent_id = ind[marker->parent];
        myfile<<i+1<<" "<<marker->type<<" "<<marker->x<<" "<<marker->y<<" "<<marker->z<<" "<<marker->radius<<" "<<parent_id<<"\n";
    }

    file.close();
    cout<<"swc file "<<fileSaveName.toStdString()<<" has been generated, size: "<<outmarkers.size()<<endl;
    return true;
};

// group images blending function

bool match_marker(V3DPluginCallback2 &callback,vector<int> &ind,LandmarkList &terafly_landmarks,LocationSimple &t)
{
    for(V3DLONG i=0;i<terafly_landmarks.size();i++)
    {
        cout<<"new marker is "<<terafly_landmarks[i].x<<"  "<<terafly_landmarks[i].y<<"  "<<terafly_landmarks[i].z<<"  "<<endl;
    }
    const Image4DSimple* curr = callback.getImageTeraFly();
    double para_ratio = curr->getRezX()/curr->getXDim();
    vector<int> loc;
    NeuronTree resultTree_2;
    loc.clear();
    resultTree_2 = callback.getSWCTeraFly();
    writeSWC_file(QString("test_swc.swc"),resultTree_2);
    //v3d_msg("check1");
    if(terafly_landmarks.size()==1)
    {
        t.x = terafly_landmarks[0].x;
        t.y = terafly_landmarks[0].y;
        t.z = terafly_landmarks[0].z;
    }
    else
    {
        if(ind.size()==1)
        {
            t.x = terafly_landmarks[ind[0]].x;
            t.y = terafly_landmarks[ind[0]].y;
            t.z = terafly_landmarks[ind[0]].z;
        }
        else
        {

            for(V3DLONG i=0;i<terafly_landmarks.size();i++)
            {
                double dis;double min_dis=100000000000;
                for(V3DLONG j=0;j<resultTree_2.listNeuron.size();j++)
                {
                    cout<<resultTree_2.listNeuron[j].x<<"  "<<terafly_landmarks[i].x<<endl;
                    dis = sqrt( (resultTree_2.listNeuron[j].x - terafly_landmarks[i].x)*(resultTree_2.listNeuron[j].x - terafly_landmarks[i].x) + (resultTree_2.listNeuron[j].y - terafly_landmarks[i].y)*(resultTree_2.listNeuron[j].y - terafly_landmarks[i].y)+(resultTree_2.listNeuron[j].z - terafly_landmarks[i].z)*(resultTree_2.listNeuron[j].z - terafly_landmarks[i].z) );
                    if(min_dis>dis)
                    {
                        min_dis = dis;
                    }
                }
                //cout<<"min_dis = "<<min_dis<<endl;
                //v3d_msg("min_dis");
                //cout<<"resultTree.listNeuron.size = "<<resultTree.listNeuron.size()<<endl;
                if(min_dis==100000000000)
                {
                    v3d_msg("resultTree_2 is void");
                    return false;
                }
                //cout<<"min______________________dis = "<<min_dis<<endl;

                //cout<<"min_dis/para = "<<min_dis/para_ratio<<endl;
                if(min_dis/para_ratio>3)  //4
                {
                    //v3d_msg("terafly_landmarks fit");
                    cout<<"terafly_landmarks fit = "<<terafly_landmarks[i].x<<"  "<<terafly_landmarks[i].y<<endl;
                    loc.push_back(i);
                }
                else
                {
                    //v3d_msg("terafly_landmarks don't fit");
                    cout<<"terafly_landmarks_don't fit = "<<terafly_landmarks[i].x<<"  "<<terafly_landmarks[i].y<<endl;
                }
                cout<<"loc.size() = "<<loc.size()<<endl;
            }

            if(loc.size()!=1)
            {
                if(loc.size()==0)
                {
                    t = simple_rebase;
                    return true;
                }
                else
                {
                    cout<<"loc.size = "<<loc.size()<<endl;
                    v3d_msg("there is a problem about markers");
                    return false;
                }
            }
            else   //loc just one
            {
                cout<<"loc[0] = "<<loc[0]<<endl;
                cout<<"ind.size = "<<ind.size()<<endl;
                if(ind.size()==0)
                {
                    v3d_msg("marker is wrong,please check it");
                    return false;
                }
                for(V3DLONG i=0;i<ind.size();i++)
                {
                    cout<<"ind"<<ind[i]<<endl;
                }
                t.x = terafly_landmarks[loc[0]].x;
                t.y = terafly_landmarks[loc[0]].y;
                t.z = terafly_landmarks[loc[0]].z;
            }
            //v3d_msg("in 2");.

        }
    }
    simple_rebase = t;
    return true;
}

bool crawler_raw_app(V3DPluginCallback2 &callback, QWidget *parent,TRACE_LS_PARA &P,bool bmenu)
{
    cout<<"%%%%%%%%%%%%%%%%%%%%%%%%%%%%%crawler raw app&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&"<<endl;
    QElapsedTimer timer1;
    timer1.start();
    bool use_comment = false;
    if(resultTree_rebase.listNeuron.size()==0)
    {
        resultTree_rebase = callback.getSWCTeraFly();
    }
    const Image4DSimple *curr_block = callback.getImageTeraFly();
    outimg_file = "X_"+QString::number(curr_block->getOriginX())+"Y_"+QString::number(curr_block->getOriginY())+"Z_"+QString::number(curr_block->getOriginZ())+".v3draw";
    QString imageSaveString = "test_app2.v3draw";
    QString swcString = outimg_file + "_app2.swc";
    QString outmarker_file =outimg_file + ".marker";
    V3DLONG data1d_sz[4];
    if(bmenu)
    {
        double center_x,center_y,center_z;
        V3DLONG xe,xb,ye,yb,ze,zb;
        V3DLONG sc = 1;
        LandmarkList terafly_landmarks = callback.getLandmarkTeraFly();
        if(P.markerfilename.isEmpty())
        {
            for(V3DLONG i=0;i<terafly_landmarks.size();i++)
            {
                center_x = terafly_landmarks[i].x;
                center_y = terafly_landmarks[i].y;
                center_z = terafly_landmarks[i].z;
            }
        }
        else
        {
            if(P.markerfilename.endsWith(".marker",Qt::CaseSensitive))
            {

                vector<MyMarker> markers;
                markers = readMarker_file(string(qPrintable(P.markerfilename)));
                for(int i = 0; i < markers.size(); i++)
                {
                    center_x = markers[i].x;
                    center_y = markers[i].y;
                    center_z = markers[i].z;
                }
            }
            else
            {
                QList<CellAPO> markers;
                markers = readAPO_file(P.markerfilename);
                for(int i = 0; i < markers.size(); i++)
                {
                    center_x = markers[i].x;
                    center_y = markers[i].y;
                    center_z = markers[i].z;
                }
            }
        }

        V3DLONG lens_x=P.block_size;
        V3DLONG lens_y=P.block_size;
        V3DLONG lens_z=P.block_size;
        xb = center_x-lens_x;
        xe = center_x+lens_x;
        yb = center_y-lens_y;
        ye = center_y+lens_y;
        zb = center_z-lens_z;
        ze = center_z+lens_z;
        data1d_sz[0] = xe - xb + 1;
        data1d_sz[1] = ye - yb + 1;
        data1d_sz[2] = ze - zb + 1;
        data1d_sz[3] = sc;
        P.o_x = xb;
        P.o_y = yb;
        P.o_z = zb;
        unsigned char *data1d=0;
        data1d = callback.getSubVolumeTeraFly(P.inimg_file.toStdString(),xb,xe+1,yb,ye+1,zb,ze+1);
        simple_saveimage_wrapper(callback, outimg_file.toStdString().c_str(),(unsigned char *)data1d,data1d_sz,1);
    }
    else
    {
        cout<<"$$$$$$$$$$$$$$$$$$$$$$get into curr window$$$$$$$$$$$$$$$$$$$$$$$$"<<endl;
        P.inimg_file = outimg_file;

        LandmarkList terafly_landmarks_terafly = callback.getLandmarkTeraFly();//terafly_landmarks
        LandmarkList new_marker;
        LocationSimple t;
        double ox = curr_block->getOriginX();
        double oy = curr_block->getOriginY();
        double oz = curr_block->getOriginZ();
        double lx = curr_block->getRezX();
        double ly = curr_block->getRezY();
        double lz = curr_block->getRezZ();
        P.ratio_x = curr_block->getRezX()/curr_block->getXDim();
        P.ratio_y = curr_block->getRezY()/curr_block->getYDim();
        P.ratio_z = curr_block->getRezZ()/curr_block->getZDim();
        P.o_x=curr_block->getOriginX();
        P.o_y=curr_block->getOriginY();
        P.o_z=curr_block->getOriginZ();

        if(terafly_landmarks_terafly.isEmpty())return false;
        LandmarkList terafly_landmarks;
        LandmarkList other_marker;
        //set<LocationSimple> terafly_landmarks;
        for(V3DLONG i=0;i<terafly_landmarks_terafly.size();i++) //rebase one is t,add in 0607,if it doesn't work or don't need it,just delete this;
        {
            LocationSimple s = terafly_landmarks_terafly[i];
            if(s.comments == "a")
            {
                t = s;
                use_comment = true;
            }
            else
            {
                marker_rebase3.push_back(s);
            }
        }
        if(use_comment == false)
        {
            for(V3DLONG i=0;i<terafly_landmarks_terafly.size();i++)
            {
                LocationSimple s = terafly_landmarks_terafly[i];
                if(s.x<ox+lx&&s.y<oy+ly&&s.z<oz+lz&&s.x>ox&&s.y>oy&&s.z>oz)
                {
                    //terafly_landmarks.insert(s);
                    //   s.comments
                    terafly_landmarks.push_back(s);
                }
                else
                {
                    other_marker.push_back(s);
                }
            }

            data1d_sz[0] = curr_block->getXDim();
            data1d_sz[1] = curr_block->getYDim();
            data1d_sz[2] = curr_block->getZDim();
            data1d_sz[3] = curr_block->getCDim();

            cout<<"marker rebase num = "<<marker_rebase.size()<<endl;
            vector<int> ind;
            for(V3DLONG i=0;i<terafly_landmarks.size();i++)
            {
                double dis;
                double min_dis = 10000000000;
                for(V3DLONG j=0;j<marker_rebase.size();j++)
                {
                    dis = (terafly_landmarks[i].x-marker_rebase[j].x)*(terafly_landmarks[i].x-marker_rebase[j].x)+(terafly_landmarks[i].y-marker_rebase[j].y)*(terafly_landmarks[i].y-marker_rebase[j].y)+(terafly_landmarks[i].z-marker_rebase[j].z)*(terafly_landmarks[i].z-marker_rebase[j].z);
                    if(dis<min_dis)
                    {
                        min_dis = dis;
                    }

                }
                //v3d_msg("out_1");
                if(min_dis>0.0001)
                {
                    ind.push_back(i);
                    new_marker.push_back(terafly_landmarks[i]);
                }
            }

            if(!match_marker(callback,ind,new_marker,t))
            {
                v3d_msg("abort");
                return false;
            }
            marker_rebase3.clear();
            for(V3DLONG i=0;i<terafly_landmarks_terafly.size();i++)
            {
                if((t.x-terafly_landmarks_terafly[i].x)*(t.x-terafly_landmarks_terafly[i].x)+(t.y-terafly_landmarks_terafly[i].y)*(t.y-terafly_landmarks_terafly[i].y)+(t.z-terafly_landmarks_terafly[i].z)*(t.z-terafly_landmarks_terafly[i].z)>0.001)
                {
                    marker_rebase3.push_back(terafly_landmarks_terafly[i]);
                }
            }
            cout<<marker_rebase3.size()<<"  "<<terafly_landmarks_terafly.size()<<endl;
            //    v3d_msg("check two size");
        }

            cout<<"t = "<<t.x<<"  "<<t.y<<"  "<<t.z<<"  "<<endl;
            cout<<"simple_rebase = "<<true_rebase.x<<"  "<<true_rebase.y<<"  "<<true_rebase.z<<"  "<<endl;
        double diff = (t.x-true_rebase.x)*(t.x-true_rebase.x)+(t.y-true_rebase.y)*(t.y-true_rebase.y)+(t.z-true_rebase.z)*(t.z-true_rebase.z);//0524
        cout<<"diff = "<<diff<<endl;
        true_rebase = t;//0524
        if(diff<0.001)
        {
            v3d_msg("the same marker!");
            change = false;
            bool miok;
            thresh = QInputDialog::getInt(0,"Intensity Threshold 1%-99%","please input your number",42,1,200,5,&miok);
            if(miok)
            {
                cout<<"input number is "<<thresh<<endl;
            }
            else
            {
                return false;
                //callback.setSWCTeraFly(resultTree_rebase);
            }
            cout<<"thresh = "<<thres_rebase<<endl;
            if(thresh != thres_rebase)
            {
                callback.setSWCTeraFly(resultTree_rebase);
            }
            else
            {
                v3d_msg("the same thresh,please try again!");
                return false;
            }
            thres_rebase = thresh;
        }
        cout<<t.x<<"  "<<t.y<<"  "<<t.z<<endl;
      //  v3d_msg("show_marker");
        //callback.setLandmarkTeraFly(marker_rebase);
        marker_rebase2 = terafly_landmarks_terafly;   // 20180522

        t.x = t.x-P.o_x;
        t.y = t.y-P.o_y;
        t.z = t.z-P.o_z;
        t.x = t.x/P.ratio_x;
        t.y = t.y/P.ratio_y;
        t.z = t.z/P.ratio_z;
        P.listLandmarks.clear();
        P.listLandmarks.push_back(t);
        cout<<"P = "<<P.listLandmarks.size()<<endl;

        QList<ImageMarker> marker;
        ImageMarker markerlist;
        markerlist.x = t.x;
        markerlist.y = t.y;
        markerlist.z = t.z;
        marker.push_back(markerlist);

        writeMarker_file(outmarker_file,marker);

        Image4DSimple *data = new Image4DSimple;
        data->setData((unsigned char *)curr_block->getRawData(),curr_block->getXDim(),curr_block->getYDim(),curr_block->getZDim(),curr_block->getCDim(),V3D_UINT8);
        data1d_sz[0] = data->getXDim();
        data1d_sz[1] = data->getYDim();
        data1d_sz[2] = data->getZDim();
        data1d_sz[3] = data->getCDim();

        simple_saveimage_wrapper(callback, outimg_file.toStdString().c_str(),(unsigned char *)data->getRawData(),data1d_sz,1);

        //bool menu_enhance = true;
        input_PARA PARA;
        QString coord_name = QString::number(curr_block->getOriginX())+QString::number(curr_block->getOriginY())+QString::number(curr_block->getOriginZ());
        PARA.inimg_file = outimg_file;
        QString str_outimg_filename = outimg_file + "_anisodiff.raw";
         fstream _file;
         _file.open(str_outimg_filename.toStdString().c_str(),ios::in);
        if(!_file)
        {
            if(enhance)
            {
                thresh = 30;
                PARA.channel = 1;
                anisodiff_func(callback,parent,PARA,bmenu);

            }
        }

        V3DLONG all_volume = data1d_sz[0]*data1d_sz[1]*data1d_sz[2];
        cout<<"all_volume = "<<all_volume<<endl;
        //v3d_msg("check all volume!");
        double c = 0;
        double d = 0;
        for(V3DLONG i=0;i<all_volume;i++)
        {

            if((int)curr_block->getRawData()[i]>thresh)
            {
                //cout<<"(int)curr_block->getRawData()[i] = "<<(int)curr_block->getRawData()[i]<<endl;
                c++;
            }
            if((int)curr_block->getRawData()[i]>60)
            {
                //cout<<"(int)curr_block->getRawData()[i] = "<<(int)curr_block->getRawData()[i]<<endl;
                d++;
            }
        }
        cout<<"c/all_volume = "<<c/all_volume<<endl;
        cout<<"d        /all_volume = "<<d/all_volume<<endl;
        //v3d_msg("check!");
        if(c/all_volume>0.3)
        {
            v3d_msg("thresh is too low!");
            return false;
        }
        if(d/all_volume>0.3)
        {
            is_soma = true;
        }
//        for(V3DLONG i=0;i<all_volume;i++)
//        {
//            if((int)curr_block->getRawData()[i]<10)
//            {
//                int last_point1 = curr_block->getRawData()[i-1];
//                int next_point1 = curr_block->getRawData()[i+1];
//                int last_point2 = curr_block->getRawData()[i-2];
//                int next_point2 = curr_block->getRawData()[i+2];
//                int average = (last_point1+last_point2+next_point1+next_point2)/4;
//            }

//        }
        PARA_APP2 p2;
        QString versionStr = "v0.001";
        p2.inmarker_file = outmarker_file;
        cout<<"p2"<<p2.inmarker_file.size()<<endl;
        if(enhance)
        {
            //p2.inimg_file = str_outimg_filename;
            p2.inimg_file = "final_img.v3draw";
        }
        else
        {
            p2.inimg_file = outimg_file;
        }
        p2.bkg_thresh = thresh;
        p2.is_gsdt = true;
        p2.b_resample = 0;
        cout<<" iiiiiiiiiiiiiiiiiiiiii = "<<thresh<<endl;
        //v3d_msg("start app2");
        p2.outswc_file =swcString;
        if(!proc_app2(callback, p2, versionStr))
        {
            return false;
        }
        //thresh=30;

    }
    enhance = false;

    //v3d_msg("app2_done");

    return true;
}

bool grid_raw_all(V3DPluginCallback2 &callback, QWidget *parent,TRACE_LS_PARA &P,bool bmenu)
{
    cout<<"this is grid_raw_all"<<endl;
    QElapsedTimer timer1;
    timer1.start();

    QString outimg_file = "test.v3draw";
    V3DLONG data1d_sz[4];
    if(bmenu)
    {
        double center_x,center_y,center_z;
        V3DLONG xe,xb,ye,yb,ze,zb;
        V3DLONG sc = 1;
        LandmarkList terafly_landmarks = callback.getLandmarkTeraFly();
        if(P.markerfilename.isEmpty())
        {
            for(V3DLONG i=0;i<terafly_landmarks.size();i++)
            {
                center_x = terafly_landmarks[i].x;
                center_y = terafly_landmarks[i].y;
                center_z = terafly_landmarks[i].z;
            }
        }
        else
        {
            if(P.markerfilename.endsWith(".marker",Qt::CaseSensitive))
            {

                vector<MyMarker> markers;
                markers = readMarker_file(string(qPrintable(P.markerfilename)));
                for(int i = 0; i < markers.size(); i++)
                {
                    center_x = markers[i].x;
                    center_y = markers[i].y;
                    center_z = markers[i].z;
                }
            }
            else
            {
                QList<CellAPO> markers;
                markers = readAPO_file(P.markerfilename);
                for(int i = 0; i < markers.size(); i++)
                {
                    center_x = markers[i].x;
                    center_y = markers[i].y;
                    center_z = markers[i].z;
                }
            }
        }

        V3DLONG lens_x=P.block_size;
        V3DLONG lens_y=P.block_size;
        V3DLONG lens_z=P.block_size;
        xb = center_x-lens_x;
        xe = center_x+lens_x;
        yb = center_y-lens_y;
        ye = center_y+lens_y;
        zb = center_z-lens_z;
        ze = center_z+lens_z;
        data1d_sz[0] = xe - xb + 1;
        data1d_sz[1] = ye - yb + 1;
        data1d_sz[2] = ze - zb + 1;
        data1d_sz[3] = sc;
        P.o_x = xb;
        P.o_y = yb;
        P.o_z = zb;
        unsigned char *data1d=0;
        data1d = callback.getSubVolumeTeraFly(P.inimg_file.toStdString(),xb,xe+1,yb,ye+1,zb,ze+1);
        simple_saveimage_wrapper(callback, outimg_file.toStdString().c_str(),(unsigned char *)data1d,data1d_sz,1);
    }
    else
    {
        const Image4DSimple *curr_block = callback.getImageTeraFly();
        data1d_sz[0] = curr_block->getXDim();
        data1d_sz[1] = curr_block->getYDim();
        data1d_sz[2] = curr_block->getZDim();
        data1d_sz[3] = curr_block->getCDim();

        P.ratio_x = curr_block->getRezX()/curr_block->getXDim();
        P.ratio_y = curr_block->getRezY()/curr_block->getYDim();
        P.ratio_z = curr_block->getRezZ()/curr_block->getZDim();
        P.o_x=curr_block->getOriginX();
        P.o_y=curr_block->getOriginY();
        P.o_z=curr_block->getOriginZ();

        simple_saveimage_wrapper(callback, outimg_file.toStdString().c_str(),(unsigned char *)curr_block->getRawData(),data1d_sz,1);

    }

    QString fileOpenName = outimg_file;

    if(fileOpenName.endsWith(".tc",Qt::CaseSensitive))
    {
        Y_VIM<REAL, V3DLONG, indexed_t<V3DLONG, REAL>, LUT<V3DLONG> > vim;

        if( !vim.y_load( P.inimg_file.toStdString()) )
        {
            printf("Wrong stitching configuration file to be load!\n");
            return false;
        }

        P.in_sz[0] = vim.sz[0];
        P.in_sz[1] = vim.sz[1];
        P.in_sz[2] = vim.sz[2];

    }else if (fileOpenName.endsWith(".raw",Qt::CaseSensitive) || fileOpenName.endsWith(".v3draw",Qt::CaseSensitive))
    {
        unsigned char * datald = 0;
        V3DLONG *in_zz = 0;
        V3DLONG *in_sz = 0;
        int datatype;
        //if (!loadRawRegion(const_cast<char *>(P.inimg_file.toStdString().c_str()), datald, in_zz, in_sz,datatype,0,0,0,1,1,1))
        if (!loadRawRegion(const_cast<char *>(fileOpenName.toStdString().c_str()), datald, in_zz, in_sz,datatype,0,0,0,1,1,1))
        {
            return false;
        }
        if(datald) {delete []datald; datald = 0;}
        P.in_sz[0] = data1d_sz[0];
        P.in_sz[1] = data1d_sz[1];
        P.in_sz[2] = data1d_sz[2];
        cout<<P.in_sz[0]<<"  "<<P.in_sz[1]<<"  "<<P.in_sz[2]<<"  "<<endl;
    }else
    {
        V3DLONG *in_zz = 0;
        if(!callback.getDimTeraFly(fileOpenName.toStdString(),in_zz))
        {
            return false;
        }
        P.in_sz[0] = in_zz[0];
        P.in_sz[1] = in_zz[1];
        P.in_sz[2] = in_zz[2];
    }

    P.inimg_file = outimg_file;
    all_tracing_grid(callback,P,P.in_sz[0],P.in_sz[1],P.in_sz[2]);
    //all_tracing_grid(callback,P,data1d_sz[0],data1d_sz[1],data1d_sz[2]);

    return true;
}

bool all_tracing_grid(V3DPluginCallback2 &callback,TRACE_LS_PARA &P,V3DLONG ix, V3DLONG iy, V3DLONG iz)
{
//    cout<<"name = "<<P.inimg_file.toStdString()<<endl;
    QString saveDirString;

    QString imageSaveString = saveDirString;

    V3DLONG start_x,start_y,start_z,end_x,end_y,end_z;
    start_x = ix;
    start_y = iy;
    start_z = iz;
    end_x = ix + P.block_size; if(end_x > P.in_sz[0]) end_x = P.in_sz[0];
    end_y = iy + P.block_size; if(end_y > P.in_sz[1]) end_y = P.in_sz[1];
    end_z = iz + P.block_size; if(end_z > P.in_sz[2]) end_z = P.in_sz[2];

    QString swcString = saveDirString;

    V3DPluginArgItem arg;
    V3DPluginArgList input;
    V3DPluginArgList output;

    QString full_plugin_name;
    QString func_name;

    arg.type = "random";
    std::vector<char*> arg_input;
    std:: string fileName_Qstring(P.inimg_file.toStdString());
    char* fileName_string =  new char[fileName_Qstring.length() + 1];
    strcpy(fileName_string, fileName_Qstring.c_str());
    //std:: string fileName_Qstring(imageSaveString.toStdString());char* fileName_string =  new char[fileName_Qstring.length() + 1]; strcpy(fileName_string, fileName_Qstring.c_str());
    arg_input.push_back(fileName_string);
    cout<<"fileName_string = "<<fileName_string<<endl;
    arg.p = (void *) & arg_input;
    input<< arg;

    char* char_swcout =  new char[swcString.length() + 1];strcpy(char_swcout, swcString.toStdString().c_str());
    arg.type = "random";std::vector<char*> arg_output;arg_output.push_back(char_swcout); arg.p = (void *) & arg_output; output<< arg;

    arg.type = "random";
    std::vector<char*> arg_para;
    if(P.method == neutube || P.method == app2)
    {
        arg_para.push_back("1");
        arg_para.push_back("1");
        full_plugin_name = "neuTube";
        func_name =  "neutube_trace";
    }else if(P.method == snake)
    {
        arg_para.push_back("1");
        arg_para.push_back("1");
        full_plugin_name = "snake";
        func_name =  "snake_trace";
    }else if(P.method == most)
    {
        string S_channel = boost::lexical_cast<string>(P.channel);
        char* C_channel = new char[S_channel.length() + 1];
        strcpy(C_channel,S_channel.c_str());
        arg_para.push_back(C_channel);

        string S_background_th = boost::lexical_cast<string>(P.bkg_thresh);
        char* C_background_th = new char[S_background_th.length() + 1];
        strcpy(C_background_th,S_background_th.c_str());
        arg_para.push_back(C_background_th);

        string S_seed_win = boost::lexical_cast<string>(P.seed_win);
        char* C_seed_win = new char[S_seed_win.length() + 1];
        strcpy(C_seed_win,S_seed_win.c_str());
        arg_para.push_back(C_seed_win);

        string S_slip_win = boost::lexical_cast<string>(P.slip_win);
        char* C_slip_win = new char[S_slip_win.length() + 1];
        strcpy(C_slip_win,S_slip_win.c_str());
        arg_para.push_back(C_slip_win);

        full_plugin_name = "mostVesselTracer";
        func_name =  "MOST_trace";
    }

    arg.p = (void *) & arg_para; input << arg;
    if(!callback.callPluginFunc(full_plugin_name,func_name,input,output))
    {
        printf("Can not find the tracing plugin!\n");
        return false;
    }
    QString swcNEUTUBE = saveDirString;

    vector<MyMarker*> inputswc;
    inputswc = readSWC_file(swcNEUTUBE.toStdString());;
    for(V3DLONG d = 0; d < inputswc.size(); d++)
    {
        inputswc[d]->x = inputswc[d]->x + start_x;
        inputswc[d]->y = inputswc[d]->y + start_y;
        inputswc[d]->z = inputswc[d]->z + start_z;

    }
    cout<<"swcString.toStdString() = "<<swcString.toStdString()<<endl;
    saveSWC_file(swcString.toStdString().c_str(), inputswc);
    system(qPrintable(QString("rm %1").arg(swcNEUTUBE.toStdString().c_str())));
    system(qPrintable(QString("rm %1").arg(imageSaveString.toStdString().c_str())));
}

QString getAppPath()
{
    QString v3dAppPath("~/Work/v3d_external/v3d");
    QDir testPluginsDir = QDir(qApp->applicationDirPath());

#if defined(Q_OS_WIN)
    if (testPluginsDir.dirName().toLower() == "debug" || testPluginsDir.dirName().toLower() == "release")
        testPluginsDir.cdUp();
#elif defined(Q_OS_MAC)
    if (testPluginsDir.dirName() == "MacOS") {
        QDir testUpperPluginsDir = testPluginsDir;
        testUpperPluginsDir.cdUp();
        testUpperPluginsDir.cdUp();
        testUpperPluginsDir.cdUp(); // like foo/plugins next to foo/v3d.app
        if (testUpperPluginsDir.cd("plugins")) testPluginsDir = testUpperPluginsDir;
        testPluginsDir.cdUp();
    }
#endif

    v3dAppPath = testPluginsDir.absolutePath();
    return v3dAppPath;
}

bool export_2dtif(V3DPluginCallback & cb,const char * filename, unsigned char * pdata, V3DLONG sz[3], int datatype)
{
    if (!filename || !sz || !pdata)
    {
        v3d_msg("some of the parameters for simple_saveimage_wrapper() are not valid.", 0);
        return false;
    }

    ImagePixelType dt;
    if (datatype==1)
        dt = V3D_UINT8;
    else if (datatype==2)
        dt = V3D_UINT16;
    else if (datatype==4)
        dt = V3D_FLOAT32;
    else
    {
        v3d_msg(QString("the specified save data type in simple_saveimage_wrapper() is not valid, dt=[%1].").arg(datatype), 0);
        return false;
    }

    Image4DSimple * outimg = new Image4DSimple;
    if (outimg)
    {
        cout<<"save"<<endl;
        outimg->setData(pdata, sz[0], sz[1], 0 ,sz[3], dt);
    }
    else
    {
        v3d_msg("Fail to new Image3DSimple for outimg.");
        return false;
    }

    return cb.saveImage(outimg, (char *)filename);
}
bool anisodiff_func(V3DPluginCallback2 &callback, QWidget *parent, input_PARA &PARA, bool bmenu)
{
    unsigned char* p_img_input = 0;
    V3DLONG sz_img_input[4];
    if(bmenu)
    {
        v3dhandle curwin = callback.currentImageWindow();
        if (!curwin)
        {
            QMessageBox::information(0, "", "You don't have any image open in the main window.");
            return false;
        }
        Image4DSimple* p4DImage = callback.getImage(curwin);
        if (!p4DImage)
        {
            QMessageBox::information(0, "", "The image pointer is invalid. Ensure your data is valid and try again!");
            return false;
        }
        if(p4DImage->getDatatype()!=V3D_UINT8)
        {
            QMessageBox::information(0, "", "Please convert the image to be UINT8 and try again!");
            return false;
        }
        if(p4DImage->getCDim()!=1)
        {
            QMessageBox::information(0, "", "The input image is not one channel image!");
            return false;
        }
        p_img_input = p4DImage->getRawData();
        sz_img_input[0] = p4DImage->getXDim();
        sz_img_input[1] = p4DImage->getYDim();
        sz_img_input[2] = p4DImage->getZDim();
        sz_img_input[3] = 1;
    }
    else
    {
        int datatype = 0;
        if (!simple_loadimage_wrapper(callback,PARA.inimg_file.toStdString().c_str(), p_img_input, sz_img_input, datatype))
        {
            fprintf (stderr, "Error happens in reading the subject file [%s]. Exit. \n",PARA.inimg_file.toStdString().c_str());
            return false;
        }
        if(PARA.channel < 1 || PARA.channel > sz_img_input[3])
        {
            fprintf (stderr, "Invalid channel number. \n");
            return false;
        }

        if(datatype !=1)
        {
            fprintf (stderr, "Please convert the image to be UINT8 and try again!\n");
            return false;
        }
    }
    printf("0. Make some backgrand pixels as 0 \n");
    V3DLONG pagesz = sz_img_input[0]*sz_img_input[1]*sz_img_input[2];
    for(V3DLONG i=0;i<pagesz;i++)
    {
        if(int(p_img_input[i])<15)
        {
            p_img_input[i]=0;
        }
    }
    //-----------------------------------------------------------------------------------------
    printf("1. Find the bounding box and crop image. \n");
    long l_boundbox_min[3],l_boundbox_max[3];//xyz
    V3DLONG sz_img_crop[4];
    long l_npixels_crop;
    unsigned char *p_img8u_crop=0;
    {
    //find bounding box
    unsigned char ***p_img8u_3d=0;
    if(!new3dpointer(p_img8u_3d,sz_img_input[0],sz_img_input[1],sz_img_input[2],p_img_input))
    {
        printf("ERROR: Fail to allocate memory for the 4d pointer of image.\n");
        if(p_img8u_3d) 				{delete3dpointer(p_img8u_3d,sz_img_input[0],sz_img_input[1],sz_img_input[2]);}
        return false;
    }

    l_boundbox_min[0]=sz_img_input[0];	l_boundbox_min[1]=sz_img_input[1];	l_boundbox_min[2]=sz_img_input[2];
    l_boundbox_max[0]=0;				l_boundbox_max[1]=0;				l_boundbox_max[2]=0;
    for(long X=0;X<sz_img_input[0];X++)
        for(long Y=0;Y<sz_img_input[1];Y++)
            for(long Z=0;Z<sz_img_input[2];Z++)
                if(p_img8u_3d[Z][Y][X]>0.1)
                {
                    if(l_boundbox_min[0]>X) l_boundbox_min[0]=X;	if(l_boundbox_max[0]<X) l_boundbox_max[0]=X;
                    if(l_boundbox_min[1]>Y) l_boundbox_min[1]=Y;	if(l_boundbox_max[1]<Y) l_boundbox_max[1]=Y;
                    if(l_boundbox_min[2]>Z) l_boundbox_min[2]=Z;	if(l_boundbox_max[2]<Z) l_boundbox_max[2]=Z;
                }
    printf(">>boundingbox: x[%ld~%ld],y[%ld~%ld],z[%ld~%ld]\n",l_boundbox_min[0],l_boundbox_max[0],
                                                               l_boundbox_min[1],l_boundbox_max[1],
                                                               l_boundbox_min[2],l_boundbox_max[2]);

    //crop image
    sz_img_crop[0]=l_boundbox_max[0]-l_boundbox_min[0]+1;
    sz_img_crop[1]=l_boundbox_max[1]-l_boundbox_min[1]+1;
    sz_img_crop[2]=l_boundbox_max[2]-l_boundbox_min[2]+1;
    sz_img_crop[3]=1;
    l_npixels_crop=sz_img_crop[0]*sz_img_crop[1]*sz_img_crop[2];

    p_img8u_crop=new(std::nothrow) unsigned char[l_npixels_crop]();
    if(!p_img8u_crop)
    {
        printf("ERROR: Fail to allocate memory for p_img32f_crop!\n");
        if(p_img8u_3d) 				{delete3dpointer(p_img8u_3d,sz_img_input[0],sz_img_input[1],sz_img_input[2]);}
        return false;
    }
    unsigned char *p_tmp=p_img8u_crop;
    for(long Z=0;Z<sz_img_crop[2];Z++)
        for(long Y=0;Y<sz_img_crop[1];Y++)
            for(long X=0;X<sz_img_crop[0];X++)
            {
                *p_tmp = p_img8u_3d[Z+l_boundbox_min[2]][Y+l_boundbox_min[1]][X+l_boundbox_min[0]];
                p_tmp++;
            }
    if(p_img8u_3d) 			{delete3dpointer(p_img8u_3d,sz_img_input[0],sz_img_input[1],sz_img_input[2]);}
    }
    //saveImage("d:/SVN/Vaa3D_source_code/v3d_external/released_plugins/v3d_plugins/anisodiffusion_littlequick/crop.raw",p_img8u_crop,sz_img_crop,1);

    //-----------------------------------------------------------------------------------------
    //convert image data type to float
    printf("2. Convert image data to float and scale to [0~255]. \n");
    float *p_img32f_crop=0;
    {
    p_img32f_crop=new(std::nothrow) float[l_npixels_crop]();
    if(!p_img32f_crop)
    {
        printf("ERROR: Fail to allocate memory for p_img32f_crop!\n");
        if(p_img8u_crop) 			{delete []p_img8u_crop;		p_img8u_crop=0;}
        if(p_img32f_crop) 			{delete []p_img32f_crop;			p_img32f_crop=0;}
        return false;
    }
    //find the maximal intensity value
    float d_maxintensity_input=0.0;
    for(long i=0;i<l_npixels_crop;i++)
        if(p_img8u_crop[i]>d_maxintensity_input)
            d_maxintensity_input=p_img8u_crop[i];
    //convert and rescale
    for(long i=0;i<l_npixels_crop;i++)
        p_img32f_crop[i]=p_img8u_crop[i]/d_maxintensity_input*255.0;
    printf(">>d_maxintensity=%.2f\n",d_maxintensity_input);
    //free input image to save memory
    //if(p_img_input) 			{delete []p_img_input;		p_img_input=0;}
    if(p_img8u_crop) 			{delete []p_img8u_crop;		p_img8u_crop=0;}
    }

    //-----------------------------------------------------------------------------------------
    //do anisotropic diffusion
    printf("3. Do anisotropic diffusion... \n");
    float *p_img32f_crop_output=0;
    if(!q_AnisoDiff3D(p_img32f_crop,sz_img_crop,p_img32f_crop_output))
    {
        printf("ERROR: q_AnisoDiff3D() return false!\n");
        if(p_img8u_crop) 			{delete []p_img8u_crop;		p_img8u_crop=0;}
        if(p_img32f_crop) 				{delete []p_img32f_crop;		p_img32f_crop=0;}
        if(p_img32f_crop_output) 		{delete []p_img32f_crop_output;	p_img32f_crop_output=0;}
        return false;
    }
    if(p_img32f_crop) 				{delete []p_img32f_crop;		p_img32f_crop=0;}

    //-----------------------------------------------------------------------------------------
    printf("4. Reconstruct processed crop image back to original size. \n");
    unsigned char *p_img8u_output=0;
    long l_npixels=sz_img_input[0]*sz_img_input[1]*sz_img_input[2]*sz_img_input[3];
    {
    p_img8u_output=new(std::nothrow) unsigned char[l_npixels]();
    if(!p_img8u_output)
    {
        printf("ERROR: Fail to allocate memory for p_img8u_output!\n");
        if(p_img32f_crop_output) 		{delete []p_img32f_crop_output;	p_img32f_crop_output=0;}
        return false;
    }
    //copy original image data to output image
    for(long i=0;i<l_npixels;i++)
        p_img8u_output[i]=p_img_input[i];

    unsigned char ***p_img8u_3d=0;
    if(!new3dpointer(p_img8u_3d,sz_img_input[0],sz_img_input[1],sz_img_input[2],p_img8u_output))
    {
        printf("ERROR: Fail to allocate memory for the 4d pointer of image.\n");
        if(p_img8u_output) 				{delete []p_img8u_output;	p_img8u_output=0;}
        if(p_img32f_crop_output) 		{delete []p_img32f_crop_output;	p_img32f_crop_output=0;}
        return false;
    }

    float *p_tmp=p_img32f_crop_output;
    for(long Z=0;Z<sz_img_crop[2];Z++)
        for(long Y=0;Y<sz_img_crop[1];Y++)
            for(long X=0;X<sz_img_crop[0];X++)
            {
                p_img8u_3d[Z+l_boundbox_min[2]][Y+l_boundbox_min[1]][X+l_boundbox_min[0]]=(unsigned char)(*p_tmp);
                p_tmp++;
            }

    if(p_img8u_3d) 	{delete3dpointer(p_img8u_3d,sz_img_input[0],sz_img_input[1],sz_img_input[2]);}
    if(p_img32f_crop_output) 		{delete []p_img32f_crop_output;	p_img32f_crop_output=0;}
    }

    //-----------------------------------------------------------------------------------------
    //save or display
    if(bmenu)
    {
        printf("5. Display the processed image in Vaa3D. \n");
        //push result image back to v3d
        v3dhandle newwin=callback.newImageWindow("output");
        Image4DSimple img4D_output;
        img4D_output.setData(p_img8u_output,sz_img_input[0],sz_img_input[1],sz_img_input[2],1,V3D_UINT8);
        callback.setImage(newwin,&img4D_output);
        callback.updateImageWindow(newwin);
        callback.open3DWindow(newwin);
    }
    else
    {
        printf("5. Save the processed image to file. \n");
        QString str_outimg_filename = PARA.inimg_file + "_anisodiff.raw";
        //saveImage(qPrintable(str_outimg_filename),p_img8u_output,sz_img_input,1);
        soma_remove_main_2(p_img8u_output,sz_img_input,callback);

        if(p_img8u_output) 		{delete []p_img8u_output;		p_img8u_output=0;}
    }

    printf(">>Program complete success!\n");
    return true;
}


//==================================================================================================
/*!
    @file

    @Copyright 2016 Numscale SAS
    @copyright 2016 J.T.Lapreste

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE1_SIMD_FUNCTION_DETAIL_PERMUTATION_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE1_SIMD_FUNCTION_DETAIL_PERMUTATION_HPP_INCLUDED

#include <boost/simd/function/simd/bitwise_cast.hpp>
#include <boost/simd/function/simd/bitwise_and.hpp>
#include <boost/simd/function/simd/make.hpp>
#include <boost/simd/swar/functions/details/default_permutation.hpp>
#include <boost/simd/swar/functions/simd/sse/sse2/details/topology.hpp>
#include <boost/simd/swar/functions/details/topology.hpp>
#include <boost/simd/swar/functions/details/permuted.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/dispatch/meta/as_floating.hpp>
#include <boost/dispatch/attributes.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/int.hpp>

namespace boost { namespace simd { namespace ext
{
  //============================================================================
  // SSE shuffle use general permutation with special case for some topology
  //============================================================================
  template<std::size_t Cardinal>
  struct sse_permutation : detail::default_permutation<Cardinal>
  {};

  //============================================================================
  // Single parameters card2 permutation are handled by _mm_shuffle_pd
  //============================================================================
  template<>
  struct  sse_permutation<2>
  {
    // For one argument: just handle zeroing
    template<typename T, typename P>
    BOOST_FORCEINLINE static T call(T const& a0, P const& p)
    {
      return call(a0,p,typename details::perform_zeroing<P,2>::type());
    }

    template<typename T, typename P>
    BOOST_FORCEINLINE static T call ( T const& a0, P const& p
                                    , boost::mpl::true_ const&
                                    )
    {
      typedef typename dispatch::meta::as_integer<T, unsigned>::type i_t;
      T val = call(a0,p,boost::mpl::false_());

      return val & make<i_t>( details::zero_mask<char,P,0,2>::value
                            , details::zero_mask<char,P,1,2>::value
                            );
    }

    template<typename T, typename P>
    BOOST_FORCEINLINE static T call ( T const& a0, P const&
                                    , boost::mpl::false_ const&
                                    )
    {
      typedef typename dispatch::meta::as_floating<T>::type f_t;
      f_t v = bitwise_cast<f_t>(a0);

      return bitwise_cast<T>( f_t(_mm_shuffle_pd (v, v
                                                 , (details::shuffle_mask<P,2>::value)
                                                 )
                                 )
                            );
    }

    // For two arguments: handle topology, all other cases are optimized already
    template<typename T, typename P>
    BOOST_FORCEINLINE static T call(T const& a0, T const& a1, P const& p)
    {
      return call(a0,a1,p,typename details::sse_topology<P,2>::type());
    }

    // Direct shuffle requires a simple call to shuffle
    template<typename T, typename P>
    BOOST_FORCEINLINE static T call ( T const& a0, T const& a1, P const&
                                    , details::direct_ const&
                                    )
    {
      typedef typename dispatch::meta::as_floating<T>::type f_t;

      return bitwise_cast<T>( f_t(_mm_shuffle_pd( bitwise_cast<f_t>(a0)
                                                , bitwise_cast<f_t>(a1)
                                                , ( details::shuffle_mask
                                                    < details::clamp<P,2>, 2>::value
                                                  )
                                                )
                                 )
                            );
    }

    // Indirect shuffle requires swapping a0/a1 in call to shuffle
    template<typename T, typename P>
    BOOST_FORCEINLINE static T call ( T const& a0, T const& a1, P const& p
                                    , details::indirect_ const&
                                    )
    {
      return call(a1,a0,p,details::direct_());
    }
  };

  //============================================================================
  // Single parameters card4 permutation are handled by _mm_shuffle_*
  //============================================================================
  template<>
  struct  sse_permutation<4>
  {
    // Do we perform zeroing ?
    template<typename T, typename P>
    BOOST_FORCEINLINE static T call(T const& a0, P const& p)
    {
      return call(a0,p,typename details::perform_zeroing<P,4>::type());
    }

    // Yes ? Mask with proper mask vector
    template<typename T, typename P>
    BOOST_FORCEINLINE static T call ( T const& a0, P const& p
                                    , boost::mpl::true_ const&
                                    )
    {
      typedef typename dispatch::meta::as_integer<T, unsigned>::type i_t;
      T val = call(a0,p,boost::mpl::false_());

      return val & make<i_t>( details::zero_mask<char,P,0,4>::value
                            , details::zero_mask<char,P,1,4>::value
                            , details::zero_mask<char,P,2,4>::value
                            , details::zero_mask<char,P,3,4>::value
                            );
    }

    // No use proper intrinsic for single
    template<typename P>
    BOOST_FORCEINLINE static __m128 call( __m128 const& a0, P const&
                                        , boost::mpl::false_ const&
                                        )
    {
      return _mm_shuffle_ps(a0, a0, (details::shuffle_mask<P,4>::value));
    }

    // No use proper intrinsic for integers
    template<typename P>
    BOOST_FORCEINLINE static __m128i call ( __m128i const& a0, P const&
                                          , boost::mpl::false_ const&
                                          )
    {
      return _mm_shuffle_epi32(a0, (details::shuffle_mask<P,4>::value));
    }

    // Two arguments check zeroing
    template<typename T, typename P>
    BOOST_FORCEINLINE static T call(T const& a0, T const& a1, P const& p)
    {
      return call(a0,a1,p,typename details::perform_zeroing<P,4>::type());
    }

    // If we mask, mask with proper mask vector
    template<typename T, typename P>
    BOOST_FORCEINLINE static T call ( T const& a0, T const& a1, P const& p
                                    , boost::mpl::true_ const&
                                    )
    {
      typedef typename dispatch::meta::as_integer<T, unsigned>::type i_t;
      T val = call(a0,a1,p,boost::mpl::false_());

      return val & make<i_t>( details::zero_mask<char,P,0,4>::value
                            , details::zero_mask<char,P,1,4>::value
                            , details::zero_mask<char,P,2,4>::value
                            , details::zero_mask<char,P,3,4>::value
                            );
    }

    // If not, inspect the permutation topology
    template<typename T, typename P>
    BOOST_FORCEINLINE static T call ( T const& a0, T const& a1, P const& p
                                    , boost::mpl::false_ const&
                                    )
    {
      return call(a0,a1,p,typename details::sse_topology<P,4>::type());
    }

    // Mixed just use regular hand made permutation
    template<typename T, typename P>
    BOOST_FORCEINLINE static T call ( T const& a0, T const& a1, P const& p
                                    , details::mixed_ const&
                                    )
    {
      return details::default_permutation<4>::call(a0,a1,p);
    }

    // Direct use _mm_shuffle_ps
    template<typename T, typename P>
    BOOST_FORCEINLINE static T call ( T const& a0, T const& a1, P const&
                                    , details::direct_ const&
                                    )
    {
      typedef typename dispatch::meta::as_floating<T>::type f_t;

      return bitwise_cast<T>( f_t(_mm_shuffle_ps( bitwise_cast<f_t>(a0)
                                                , bitwise_cast<f_t>(a1)
                                                , (details::shuffle_mask
                                                    <details::clamp<P,4>, 4>::value
                                                  )
                                                )
                                 )
                            );
    }

    // Indirect is direct with a0/a1 swapped
    template<typename T, typename P>
    BOOST_FORCEINLINE static T call ( T const& a0, T const& a1, P const& p
                                    , details::indirect_ const&
                                    )
    {
      return call(a1,a0,p,details::direct_());
    }
  };

#endif


#include "omnicore/sp.h"

#include "test/test_bitcoin.h"

#include <boost/test/unit_test.hpp>

#include <stdint.h>
#include <limits>
#include <utility>

BOOST_FIXTURE_TEST_SUITE(omnicore_crowdsale_participation_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(overpayment_close)
{

    uint16_t   precision = 2;
    int64_t amountInvested = 300000000;
    uint8_t earlyBirdBonus = 10;
    int64_t deadline = 1535644800;
    int64_t timestamp = 1407877014LL;
    int64_t amountPerUnitInvested = 10000000000;
    int soldTokens = 0;
    int64_t totalTokens = 200000000;
    int64_t purchasedTokens = 0;
    int64_t refund = 0;
    bool fClosed = false;

    //txid : db7782198b2640aa8891453fd5077968d1642bfa5947f839396ce77f8e51d286
    mastercore::calculateFundraiser(precision, amountInvested, earlyBirdBonus, deadline,
                                    timestamp, amountPerUnitInvested, soldTokens , totalTokens, purchasedTokens,
                                    fClosed, refund);

    BOOST_CHECK(!fClosed);
}

/*
 * void mastercore::calculateFundraiser(uint16_t tokenPrecision, int64_t transfer,
                                     uint8_t bonusPerc, int64_t closeSeconds,
                                     int64_t currentSeconds, int64_t price,
                                     int64_t soldTokens, int64_t totalTokens,
                                     int64_t &purchasedTokens,
                                     bool &closeCrowdsale, int64_t &refund) {
 * */

#if 0
BOOST_AUTO_TEST_CASE(max_limits)
{
    int64_t amountPerUnitInvested = std::numeric_limits<int64_t>::max();
    int64_t deadline = std::numeric_limits<int64_t>::max();
    uint8_t earlyBirdBonus = std::numeric_limits<uint8_t>::max();
    uint8_t issuerBonus = std::numeric_limits<uint8_t>::max();

    int64_t timestamp = 0;
    int64_t amountInvested = std::numeric_limits<int64_t>::max();

    int64_t totalTokens = std::numeric_limits<int64_t>::max() - 1LL;
    std::pair<int64_t, int64_t> tokensCreated;
    bool fClosed = false;

    mastercore::calculateFundraiser(true, amountInvested, earlyBirdBonus, deadline,
            timestamp, amountPerUnitInvested, issuerBonus, totalTokens,
            tokensCreated, fClosed);

    BOOST_CHECK(fClosed);
    BOOST_CHECK_EQUAL(1, tokensCreated.first);  // user
    BOOST_CHECK_EQUAL(0, tokensCreated.second); // issuer
}

BOOST_AUTO_TEST_CASE(negative_time)
{
    int64_t amountPerUnitInvested = 50;
    int64_t deadline = 500000000;
    uint8_t earlyBirdBonus = 255;
    uint8_t issuerBonus = 19;

    int64_t timestamp = 500007119;
    int64_t amountInvested = 1000000000L;

    int64_t totalTokens = 0;
    std::pair<int64_t, int64_t> tokensCreated;
    bool fClosed = false;

    mastercore::calculateFundraiser(false, amountInvested, earlyBirdBonus, deadline,
            timestamp, amountPerUnitInvested, issuerBonus, totalTokens,
            tokensCreated, fClosed);

    BOOST_CHECK(!fClosed);
    BOOST_CHECK_EQUAL(500, tokensCreated.first); // user
    BOOST_CHECK_EQUAL(95, tokensCreated.second); // issuer
}
#endif

BOOST_AUTO_TEST_SUITE_END()


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

#include "maxblocksize.h"
#include "chain.h"
#include "chainparams.h"
#include "options.h"
#include "test/test_bitcoin.h"
#include "test/testutil.h"

#include <boost/test/unit_test.hpp>

#include <algorithm>

BOOST_FIXTURE_TEST_SUITE(maxblocksize_tests, BasicTestingSetup)

void fillBlockIndex(
        Consensus::Params& params, std::vector<CBlockIndex>& blockIndexes,
        bool addVotes, int64_t currMax) {

    auto customizeFunc = [addVotes, currMax](CBlockIndex& index) {
        index.nMaxBlockSize = currMax;

        if (!addVotes) return;

        index.nMaxBlockSizeVote = std::max(index.nHeight * 1000000, 1000000);
    };
    BuildDummyBlockIndex(blockIndexes, customizeFunc);
};

BOOST_AUTO_TEST_CASE(get_next_max_blocksize) {
    auto params = Params(CBaseChainParams::REGTEST).GetConsensus();
    BOOST_CHECK_EQUAL(uint32_t(1512), params.nMaxBlockSizeChangePosition);

    // Genesis block, legacy block size
    BOOST_CHECK_EQUAL(MAX_BLOCK_SIZE, GetNextMaxBlockSize(nullptr, params));

    const int64_t interval = params.nMaxBlockSizeAdjustmentInterval;

    // Not at a difficulty adjustment interval,
    // should not change max block size.
    {
        uint64_t currMax = MAX_BLOCK_SIZE;
        std::vector<CBlockIndex> blockInterval(interval);
        fillBlockIndex(params, blockInterval, true, currMax);
        CBlockIndex index;

        for (auto& b : blockInterval) {
            if ((b.nHeight + 1) % interval == 0)
                continue;
            BOOST_CHECK_EQUAL(currMax,
                GetNextMaxBlockSize(&b, params));

        }
    }

    // No block voted. Keep current size.
    {
        uint64_t currMax = 2000000;
        std::vector<CBlockIndex> blockInterval(interval);
        fillBlockIndex(params, blockInterval, false, currMax);

        BOOST_CHECK_EQUAL(currMax,
                GetNextMaxBlockSize(&blockInterval.back(), params));
    }

    // Everyone votes current size. Keep current size.
    {
        uint64_t currMax = 2000000;
        std::vector<CBlockIndex> blockInterval(interval);
        fillBlockIndex(params, blockInterval, false, currMax);

        for (CBlockIndex& b : blockInterval)
            b.nMaxBlockSizeVote = currMax;

        BOOST_CHECK_EQUAL(currMax,
                GetNextMaxBlockSize(&blockInterval.back(), params));
    }

    // Everyone votes.
    // Blocks vote (vote# * 1MB)
    {
        // Test raise.
        uint64_t currMax = 2000000;
        std::vector<CBlockIndex> blockInterval(interval);
        fillBlockIndex(params, blockInterval, true, currMax);
        uint64_t newLimit = GetNextMaxBlockSize(&blockInterval.back(), params);
        BOOST_CHECK_EQUAL(uint64_t(currMax * 1.05), newLimit);

        // Test lower.
        currMax = 1000 * 2000000;
        fillBlockIndex(params, blockInterval, true, currMax);
        newLimit = GetNextMaxBlockSize(&blockInterval.back(), params);
        BOOST_CHECK_EQUAL(uint64_t(currMax / 1.05), newLimit);
    }
}

std::vector<unsigned char> to_uchar(const std::string& coinbaseStr) {
    return std::vector<unsigned char>(begin(coinbaseStr), end(coinbaseStr));
}

// If we have an explicit /B/ vote, we read it and ignore /EB/.
BOOST_AUTO_TEST_CASE(get_max_blocksize_vote_b) {

    std::vector<unsigned char> vote(to_uchar("/BIP100/B2/EB1/"));
    int32_t height = 600000;

    // Coinbase as in the internal miner
    CScript coinbase = CScript() << height << vote << OP_0;
    BOOST_CHECK_EQUAL(2000000u, GetMaxBlockSizeVote(coinbase, height));

    // Coinbase as created with IncrementExtraNonce
    unsigned int nonce = 1;
    CScript coinbase_flags;
    coinbase = (CScript() << height << vote << CScriptNum(nonce)) + coinbase_flags;
    BOOST_CHECK_EQUAL(2000000u, GetMaxBlockSizeVote(coinbase, height));

    // coinbase without height should also work
    coinbase = (CScript() << vote << CScriptNum(nonce)) + coinbase_flags;
    BOOST_CHECK_EQUAL(2000000u, GetMaxBlockSizeVote(coinbase, height));

    // can't vote twice, only first one counts.
    coinbase = (CScript() << to_uchar("/BIP100/B4/EB6/BIP100/B8/"));
    BOOST_CHECK_EQUAL(4000000u, GetMaxBlockSizeVote(coinbase, height));

    // B-votes override EB, even though EB is first.
    coinbase = (CScript() << to_uchar("/EB6/BIP100/B8/"));
    BOOST_CHECK_EQUAL(8000000u, GetMaxBlockSizeVote(coinbase, height));
}

// If /B/ is not present, we count /EB/ as a vote.
BOOST_AUTO_TEST_CASE(get_max_blocksize_vote_eb) {
    std::vector<unsigned char> vote(to_uchar("/some data/EB1/"));
    int32_t height = 600000;

    CScript coinbase = CScript() << height << vote << OP_0;
    BOOST_CHECK_EQUAL(1000000u, GetMaxBlockSizeVote(coinbase, height));

    unsigned int nonce = 1;
    CScript coinbase_flags;
    coinbase = (CScript() << height << vote << CScriptNum(nonce)) + coinbase_flags;
    BOOST_CHECK_EQUAL(1000000u, GetMaxBlockSizeVote(coinbase, height));

    // Example of a Bitcoin Unlimited coinbase string
    coinbase = CScript() << height << to_uchar("/EB16/AD12/a miner comment");
    BOOST_CHECK_EQUAL(16000000u, GetMaxBlockSizeVote(coinbase, height));

    // can't vote twice, only first one counts.
    coinbase = (CScript() << to_uchar("some data/EB6/EB8/"));
    BOOST_CHECK_EQUAL(6000000u, GetMaxBlockSizeVote(coinbase, height));
}

BOOST_AUTO_TEST_CASE(get_max_blocksize_vote_no_vote) {
    int32_t height = 600000;
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << OP_0, height));

    // votes must begin and end with /
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("/EB2"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("EB2/"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("/BIP100/B2"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("BIP100/B2/"), height));

    // whitespace is not allowed
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("/ EB2/"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("/EB2 /"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("/EB 2/"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("/BIP100/B2 /"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("/BIP100/ B2/"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("/BIP100/B 2/"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("/BIP100 /B2/"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("/ BIP100/B2/"), height));

    // decimals not supported
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("/EB2.2/"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << height << to_uchar("/BIP100/B2.2/"), height));

    // missing mb value
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << to_uchar("/BIP100/B/"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << to_uchar("/EB/"), height));

    // missing BIP100 prefix
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << to_uchar("/B2/"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << to_uchar("/BIP100/B/B8/"), height));

    //Explicit zeros and garbage
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << to_uchar("/BIP100/B0/BIP100/B2"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << to_uchar("/EB0/EB2/"), height));
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(CScript() << to_uchar("/BIP100/Bgarbage/B2/"), height));
    BOOST_CHECK_EQUAL(2000000u, GetMaxBlockSizeVote(CScript() << to_uchar("/EBgarbage/EB2/"), height));

    // Test that height is not a part of the vote string.
    // Encoded height in this test ends with /.
    // Should not be interpreted as /BIP100/B8/
    CScript coinbase = CScript() << 47;
    BOOST_CHECK_EQUAL('/', coinbase.back());
    std::vector<unsigned char> vote = to_uchar("BIP100/B8/");
    coinbase.insert(coinbase.end(), vote.begin(), vote.end()); // insert instead of << to avoid size being prepended
    BOOST_CHECK_EQUAL(0u, GetMaxBlockSizeVote(coinbase, 47));
}

BOOST_AUTO_TEST_CASE(next_block_raise_cap) {
    BOOST_CHECK_EQUAL(MAX_BLOCK_SIZE * 105 / 100, NextBlockRaiseCap(MAX_BLOCK_SIZE));
    BOOST_CHECK_THROW(NextBlockRaiseCap(MAX_BLOCK_SIZE - 1), std::invalid_argument);
}

BOOST_AUTO_TEST_SUITE_END();


<<<<<<< HEAD
/*    Copyright (c) 2010-2018, Delft University of Technology
=======
/*    Copyright (c) 2010-2019, Delft University of Technology
>>>>>>> origin/master
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#include <boost/make_shared.hpp>
#include "Tudat/Astrodynamics/Ephemerides/simpleRotationalEphemeris.h"
#include "Tudat/Astrodynamics/Ephemerides/fullPlanetaryRotationModel.h"

#if USE_CSPICE
#include "Tudat/External/SpiceInterface/spiceRotationalEphemeris.h"
#endif

#include "Tudat/SimulationSetup/EnvironmentSetup/createRotationModel.h"

#if USE_SOFA
#include "Tudat/Astrodynamics/Ephemerides/itrsToGcrsRotationModel.h"
#include "Tudat/Astrodynamics/Ephemerides/synchronousRotationalEphemeris.h"
#include "Tudat/Astrodynamics/EarthOrientation/earthOrientationCalculator.h"
#include "Tudat/Astrodynamics/EarthOrientation/shortPeriodEarthOrientationCorrectionCalculator.h"
#include "Tudat/Mathematics/Interpolators/jumpDataLinearInterpolator.h"
#endif

namespace tudat
{

namespace simulation_setup
{

//! Function to retrieve a state from one of two functions
Eigen::Vector6d getStateFromSelectedStateFunction(
        const double currentTime,
        const bool useFirstFunction,
        const std::function< Eigen::Vector6d( const double ) > stateFunction1,
        const std::function< Eigen::Vector6d( const double ) > stateFunction2 )
{
    return ( useFirstFunction ) ? ( stateFunction1( currentTime ) ) : ( stateFunction2( currentTime ) );
}

//! Function to create a state function for a body, valid both during propagation, and outside propagation
std::function< Eigen::Vector6d( const double, bool ) > createRelativeStateFunction(
        const NamedBodyMap& bodyMap,
        const std::string orbitingBody,
        const std::string centralBody )
{
    // Retrieve state functions for relevant bodies (obtained from current state of body objects)
    std::function< Eigen::Vector6d( const double ) > bodyInertialStateFunction =
            std::bind( &Body::getState, bodyMap.at( orbitingBody ) );
    std::function< Eigen::Vector6d( const double ) > centralBodyInertialStateFunction =
            std::bind( &Body::getState, bodyMap.at(  centralBody ) );

    // Define relative state function from body object
    std::function< Eigen::Vector6d( const double ) > fromBodyStateFunction =
            std::bind(
                &ephemerides::getDifferenceBetweenStates, bodyInertialStateFunction,
                centralBodyInertialStateFunction, std::placeholders::_1 );

    // Define state function from ephemeris
    std::function< Eigen::Vector6d( const double ) > fromEphemerisStateFunction;

    if( bodyMap.at( orbitingBody )->getEphemeris( )->getReferenceFrameOrigin( ) == centralBody )
    {
        fromEphemerisStateFunction = std::bind( &ephemerides::Ephemeris::getCartesianState,
                                                bodyMap.at( orbitingBody )->getEphemeris( ), std::placeholders::_1 );

    }
    else
    {
        std::function< Eigen::Vector6d( const double ) > ephemerisInertialStateFunction =
                std::bind( &Body::getStateInBaseFrameFromEphemeris< double, double >, bodyMap.at( orbitingBody ),
                           std::placeholders::_1 );
        std::function< Eigen::Vector6d( const double ) > ephemerisCentralBodyInertialStateFunction =
                std::bind( &Body::getStateInBaseFrameFromEphemeris< double, double >, bodyMap.at( centralBody ),
                           std::placeholders::_1 );
        fromEphemerisStateFunction = std::bind(
                    &ephemerides::getDifferenceBetweenStates,
                    ephemerisInertialStateFunction,
                    ephemerisCentralBodyInertialStateFunction, std::placeholders::_1 );
    }

    return std::bind( &getStateFromSelectedStateFunction, std::placeholders::_1, std::placeholders::_2,
                      fromBodyStateFunction, fromEphemerisStateFunction );
}

//! Function to create a rotation model.
std::shared_ptr< ephemerides::RotationalEphemeris > createRotationModel(
        const std::shared_ptr< RotationModelSettings > rotationModelSettings,
        const std::string& body,
        const NamedBodyMap& bodyMap )
{
    using namespace tudat::ephemerides;

    // Declare return object.
    std::shared_ptr< RotationalEphemeris > rotationalEphemeris;

    // Check which type of rotation model is to be created.
    switch( rotationModelSettings->getRotationType( ) )
    {
    case simple_rotation_model:
    {
        // Check whether settings for simple rotation model are consistent with its type.
        std::shared_ptr< SimpleRotationModelSettings > simpleRotationSettings =
                std::dynamic_pointer_cast< SimpleRotationModelSettings >( rotationModelSettings );
        if( simpleRotationSettings == nullptr )
        {
            throw std::runtime_error(
                        "Error, expected simple rotation model settings for " + body );
        }
        else
        {
            // Create and initialize simple rotation model.
            rotationalEphemeris = std::make_shared< SimpleRotationalEphemeris >(
                        simpleRotationSettings->getInitialOrientation( ),
                        simpleRotationSettings->getRotationRate( ),
                        simpleRotationSettings->getInitialTime( ),
                        simpleRotationSettings->getOriginalFrame( ),
                        simpleRotationSettings->getTargetFrame( ) );
        }
        break;
    }
#if USE_SOFA
    case gcrs_to_itrs_rotation_model:
    {

        // Check whether settings for simple rotation model are consistent with its type.
        std::shared_ptr< GcrsToItrsRotationModelSettings > gcrsToItrsRotationSettings =
                std::dynamic_pointer_cast< GcrsToItrsRotationModelSettings >( rotationModelSettings );
        if( gcrsToItrsRotationSettings == nullptr )
        {
            throw std::runtime_error(
                        "Error, expected GCRS to ITRS rotation model settings for " + body );
        }
        else
        {
            std::shared_ptr< earth_orientation::EOPReader > eopReader = std::make_shared< earth_orientation::EOPReader >(
                        gcrsToItrsRotationSettings->getEopFile( ),
                        gcrsToItrsRotationSettings->getEopFileFormat( ),
                        gcrsToItrsRotationSettings->getNutationTheory( ) );

            // Load polar motion corrections
            std::shared_ptr< interpolators::LinearInterpolator< double, Eigen::Vector2d > > cipInItrsInterpolator =
                    std::make_shared< interpolators::LinearInterpolator< double, Eigen::Vector2d > >(
                        eopReader->getCipInItrsMapInSecondsSinceJ2000( ) );

            // Load nutation corrections
            std::shared_ptr< interpolators::LinearInterpolator< double, Eigen::Vector2d > > cipInGcrsCorrectionInterpolator =
                    std::make_shared< interpolators::LinearInterpolator< double, Eigen::Vector2d > >(
                        eopReader->getCipInGcrsCorrectionMapInSecondsSinceJ2000( ) );

            // Create polar motion correction (sub-diural frequencies) object
            std::shared_ptr< earth_orientation::ShortPeriodEarthOrientationCorrectionCalculator< Eigen::Vector2d > >
                    shortPeriodPolarMotionCalculator =
                    std::make_shared< earth_orientation::ShortPeriodEarthOrientationCorrectionCalculator< Eigen::Vector2d > >(
                        gcrsToItrsRotationSettings->getPolarMotionCorrectionSettings( )->conversionFactor_,
                        gcrsToItrsRotationSettings->getPolarMotionCorrectionSettings( )->minimumAmplitude_,
                        gcrsToItrsRotationSettings->getPolarMotionCorrectionSettings( )->amplitudesFiles_,
                        gcrsToItrsRotationSettings->getPolarMotionCorrectionSettings( )->argumentMultipliersFile_ );

            // Create full polar motion calculator
            std::shared_ptr< earth_orientation::PolarMotionCalculator > polarMotionCalculator =
                    std::make_shared< earth_orientation::PolarMotionCalculator >
                    ( cipInItrsInterpolator, shortPeriodPolarMotionCalculator );

            // Create IAU 2006 precession/nutation calculator
            std::shared_ptr< earth_orientation::PrecessionNutationCalculator > precessionNutationCalculator =
                    std::make_shared< earth_orientation::PrecessionNutationCalculator >(
                        gcrsToItrsRotationSettings->getNutationTheory( ), cipInGcrsCorrectionInterpolator );

            // Create UT1 correction (sub-diural frequencies) object
            std::shared_ptr< earth_orientation::ShortPeriodEarthOrientationCorrectionCalculator< double > >
                    ut1CorrectionSettings =
                    std::make_shared< earth_orientation::ShortPeriodEarthOrientationCorrectionCalculator< double > >(
                        gcrsToItrsRotationSettings->getUt1CorrectionSettings( )->conversionFactor_,
                        gcrsToItrsRotationSettings->getUt1CorrectionSettings( )->minimumAmplitude_,
                        gcrsToItrsRotationSettings->getUt1CorrectionSettings( )->amplitudesFiles_,
                        gcrsToItrsRotationSettings->getUt1CorrectionSettings( )->argumentMultipliersFile_ );

            std::shared_ptr< interpolators::OneDimensionalInterpolator < double, double > > dailyUtcUt1CorrectionInterpolator =
                    std::make_shared< interpolators::JumpDataLinearInterpolator< double, double > >(
                        eopReader->getUt1MinusUtcMapInSecondsSinceJ2000( ), 0.5, 1.0 );

            // Create default time scale converter
            std::shared_ptr< earth_orientation::TerrestrialTimeScaleConverter > terrestrialTimeScaleConverter =
                    std::make_shared< earth_orientation::TerrestrialTimeScaleConverter >
                    (  dailyUtcUt1CorrectionInterpolator, ut1CorrectionSettings );

            // Create rotation model
            std::shared_ptr< earth_orientation::EarthOrientationAnglesCalculator > earthOrientationCalculator =
                    std::make_shared< earth_orientation::EarthOrientationAnglesCalculator >(
                        polarMotionCalculator, precessionNutationCalculator, terrestrialTimeScaleConverter );
            rotationalEphemeris = std::make_shared< ephemerides::GcrsToItrsRotationModel >(
                        earthOrientationCalculator, gcrsToItrsRotationSettings->getInputTimeScale( ),
                        gcrsToItrsRotationSettings->getOriginalFrame( ) );

            break;
        }

    }
#endif

#if USE_CSPICE
    case spice_rotation_model:
    {
        // Create rotational ephemeris directly from Spice.
        rotationalEphemeris = std::make_shared< SpiceRotationalEphemeris >(
                    rotationModelSettings->getOriginalFrame( ),
                    rotationModelSettings->getTargetFrame( ) );
        break;
    }
#endif
<<<<<<< HEAD
=======
    case planetary_rotation_model:
    {
        std::shared_ptr< PlanetaryRotationModelSettings > planetaryRotationModelSettings =
                std::dynamic_pointer_cast< PlanetaryRotationModelSettings >( rotationModelSettings );
        if( planetaryRotationModelSettings == nullptr )
        {
            std::cerr<<"Error, expected planetary rotation model settings for "<<body<<std::endl;
        }
        else
        {

            Eigen::VectorXd bodyKeplerElements = orbital_element_conversions::convertCartesianToKeplerianElements(
                        spice_interface::getBodyCartesianStateAtEpoch(
                            body, planetaryRotationModelSettings->getCentralBody( ), "ECLIPJ2000", "NONE", 0.0 ),
                        spice_interface::getBodyGravitationalParameter( planetaryRotationModelSettings->getCentralBody( ) ) );

            double meanAnomalyAtJ2000 = orbital_element_conversions::convertEccentricAnomalyToMeanAnomaly(
                        orbital_element_conversions::convertTrueAnomalyToEccentricAnomaly(
                            bodyKeplerElements( 5 ), bodyKeplerElements( 1 ) ), bodyKeplerElements( 1 ) );

            double meanMotion = orbital_element_conversions::convertSemiMajorAxisToEllipticalMeanMotion(
                        bodyKeplerElements( 0 ), spice_interface::getBodyGravitationalParameter(
                            planetaryRotationModelSettings->getCentralBody( ) ) + spice_interface::getBodyGravitationalParameter( body ) );

            std::shared_ptr< PlanetaryOrientationAngleCalculator > planetaryOrientationAnglesCalculator
                    = std::make_shared< PlanetaryOrientationAngleCalculator >(
                        planetaryRotationModelSettings->getAnglePsiAtEpoch( ),
                        planetaryRotationModelSettings->getAnglePsiRateAtEpoch( ),
                        planetaryRotationModelSettings->getAngleIAtEpoch( ),
                        planetaryRotationModelSettings->getAngleIRateAtEpoch( ),
                        planetaryRotationModelSettings->getAnglePhiAtEpoch( ),
                        planetaryRotationModelSettings->getAnglePhiRateAtEpoch( ),
                        planetaryRotationModelSettings->getCoreFactor( ),
                        planetaryRotationModelSettings->getFreeCoreNutationRate( ),
                        meanMotion, meanAnomalyAtJ2000,
                        planetaryRotationModelSettings->getOriginalFrame( ),
                        planetaryRotationModelSettings->getMeanMotionDirectNutationCorrections( ),
                        planetaryRotationModelSettings->getMeanMotionTimeDependentPhaseNutationCorrections( ),
                        planetaryRotationModelSettings->getTimeDependentPhaseCorrectionFunctions( ),
                        planetaryRotationModelSettings->getRotationRateCorrections( ),
                        planetaryRotationModelSettings->getxPolarMotionCoefficients( ),
                        planetaryRotationModelSettings->getyPolarMotionCoefficients( ) );

            rotationalEphemeris = std::make_shared< PlanetaryRotationModel >(
                        planetaryRotationModelSettings->getAngleN( ),
                        planetaryRotationModelSettings->getAngleJ( ),
                        planetaryOrientationAnglesCalculator,
                        planetaryRotationModelSettings->getOriginalFrame( ), planetaryRotationModelSettings->getTargetFrame( ) );
        }

        break;
    }
>>>>>>> origin/master
    case synchronous_rotation_model:
    {
        std::shared_ptr< SynchronousRotationModelSettings > synchronousRotationSettings =
                std::dynamic_pointer_cast< SynchronousRotationModelSettings >( rotationModelSettings );
        if( synchronousRotationSettings == NULL )
        {
            throw std::runtime_error( "Error, expected synchronous rotation model settings for " + body );
        }
        else
        {
            if( bodyMap.at( body )->getEphemeris( )->getReferenceFrameOrigin( ) == synchronousRotationSettings->getCentralBodyName( ) )
            {
                if( bodyMap.at( body )->getEphemeris( )->getReferenceFrameOrientation( ) !=
                        synchronousRotationSettings->getOriginalFrame( ) )
                {
                    throw std::runtime_error( "Error, ephemeris of body " + body + " is in " +
                                              bodyMap.at( body )->getEphemeris( )->getReferenceFrameOrientation( ) +
                                              " frame when making synchronous rotation model, expected " +
                                              synchronousRotationSettings->getOriginalFrame( ) + " frame." );
                }
            }
            std::shared_ptr< SynchronousRotationalEphemeris > synchronousRotationalEphemeris = std::make_shared< SynchronousRotationalEphemeris >(
                        createRelativeStateFunction( bodyMap, body, synchronousRotationSettings->getCentralBodyName( ) ),
                        synchronousRotationSettings->getCentralBodyName( ),
                        synchronousRotationSettings->getOriginalFrame( ),
                        synchronousRotationSettings->getTargetFrame( ) );

            rotationalEphemeris = synchronousRotationalEphemeris;
        }
        break;
    }
    default:
        throw std::runtime_error(
                    "Error, did not recognize rotation model settings type " +
                    std::to_string( rotationModelSettings->getRotationType( ) ) );
    }

    return rotationalEphemeris;
}

} // namespace simulation_setup

} // namespace tudat


#define EIGENLINEARSOLVERS_EIGENEIGENSIMPLICIALLDLT_CRS_CPP
#include <EigenLinearSolvers/EigenSimplicialLDLT[CRS].h>
#include <EigenLinearSolvers/EigenDirectSparseSolver[CRS].inl>

#include <sofa/core/ObjectFactory.h>

namespace EigenLinearSolvers
{
template class SOFA_EIGENLINEARSOLVERS_API EigenSimplicialLDLT< sofa::linearalgebra::CompressedRowSparseMatrix<SReal>, sofa::linearalgebra::FullVector<SReal> >;
template class SOFA_EIGENLINEARSOLVERS_API EigenSimplicialLDLT<
    sofa::linearalgebra::CompressedRowSparseMatrix< sofa::type::Mat<3,3,SReal> >,
    sofa::linearalgebra::FullVector<SReal> >;

int EigenSimplicialLDLTCRSClass = sofa::core::RegisterObject("Direct Linear Solver using a Sparse LDL^T factorization.")
.add< EigenSimplicialLDLT< sofa::linearalgebra::CompressedRowSparseMatrix<SReal>, sofa::linearalgebra::FullVector<SReal> > >()
.add< EigenSimplicialLDLT< sofa::linearalgebra::CompressedRowSparseMatrix< sofa::type::Mat<3,3,SReal> >, sofa::linearalgebra::FullVector<SReal> > >()
;

}


#include <boost/units/systems/cgs/momentum.hpp>