#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
//#include"TaskSpace.h"
#include"taskspaceoffline.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
//#include <link.h>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include <gazebo_msgs/LinkStates.h>
#include "QTime"

using namespace  std;
using namespace  Eigen;
//data of left foot sensor
int a;
int b;
int c;
int d;
int e;
int f;
int g;
int h;

void receiveFootSensor(const std_msgs::Int32MultiArray& msg)
{
    if (msg.data.size()!=8) {
        qDebug("the size of sensor data is in wrong");
        return;
    }

    ROS_INFO("I heard: [%d  %d %d %d %d  %d %d %d]", (int)msg.data[0],(int)msg.data[1],(int)msg.data[2],(int)msg.data[3],(int)msg.data[4],(int)msg.data[5],(int)msg.data[6],(int)msg.data[7]);
    double temp[8];
    int tempInt[8];
    //Left Foot
    temp[0]=msg.data[0]-1023;
    temp[1]=-1*(msg.data[1]-929);
    temp[2]=msg.data[2]-3035;
    temp[3]=-1*(msg.data[3]-3099);
    //Right Foot
    temp[4]=msg.data[4]-3041;
    temp[5]=-1*(msg.data[5]-3005);
    temp[6]=msg.data[6]-1138;
    temp[7]=-1*(msg.data[7]-1006);

    //normalizing data of sensors
    temp[0]=temp[0]*(100.0/105);
    temp[1]=temp[1]*(100.0/100);
    temp[2]=temp[2]*(100.0/117);
    temp[3]=temp[3]*(100.0/118);
    temp[4]=temp[4]*(100.0/109);
    temp[5]=temp[5]*(100.0/115);
    temp[6]=temp[6]*(100.0/107);
    temp[7]=temp[7]*(100.0/107);

    tempInt[0]=temp[0];
    tempInt[1]=temp[1];
    tempInt[2]=temp[2];
    tempInt[3]=temp[3];
    tempInt[4]=temp[4];
    tempInt[5]=temp[5];
    tempInt[6]=temp[6];
    tempInt[7]=temp[7];

    a=tempInt[0];
    b=tempInt[1];
    c=tempInt[2];
    d=tempInt[3];
    e=tempInt[4];
    f=tempInt[5];
    g=tempInt[6];
    h=tempInt[7];
    ROS_INFO("I heard: [%d  %d %d %d %d %d %d %d]", a,b,c,d,e,f,g,h);
    //deleting data with negative sign
    if (a<0) {
        a=0;

    }
    if (b<0) {
        b=0;

    }
    if (c<0) {
        c=0;

    }
    if (d<0) {
        d=0;

    }
    if (e<0) {
        e=0;

    }
    if (f<0) {
        f=0;

    }
    if (g<0) {
        g=0;

    }
    if (h<0) {
        h=0;

    }

    // ROS_INFO("I heard: [%d  %d %d %d]", tempInt[0],tempInt[1],tempInt[2],tempInt[3]);
}

//int a,b,c,d,e,f,g,h;

MatrixXd quater2rot(double w,double x,double y, double z){
    MatrixXd R(3,3);
    R<<w*w+x*x-y*y-z*z,2*x*y-2*w*z,2*x*z+2*w*y,
            2*x*y+2*w*z,w*w-x*x+y*y-z*z,2*y*z-2*w*x,
            2*x*z-2*w*y,2*y*z+2*w*x,w*w-x*x-y*y+z*z;
    return R;

}

void ankle_states(const gazebo_msgs::LinkStates& msg){
    double x_left, x_right,y_left, y_right,z_left, z_right;
Vector3d vec_A_E;
Vector3d vec_B_F;
Vector3d vec_C_G;
Vector3d vec_D_H;

vec_A_E<<-.085,
         -.08,
        -.11;
vec_B_F<<.135,
         -.08,
        -.11;
vec_C_G<<.135,
         .08,
        -.11;
vec_D_H<<-.085,
         .08,
        -.11;

    x_left=msg.pose[7].position.x;
    x_right=msg.pose[13].position.x;
    y_left=msg.pose[7].position.y;
    y_right=msg.pose[13].position.y;

    z_left=msg.pose[7].position.z;
    z_right=msg.pose[13].position.z;

    MatrixXd R_left(3,3);
    MatrixXd R_right(3,3);
    R_left=quater2rot(msg.pose[7].orientation.w,msg.pose[7].orientation.x,msg.pose[7].orientation.y,msg.pose[7].orientation.z);
    R_right=quater2rot(msg.pose[13].orientation.w,msg.pose[13].orientation.x,msg.pose[13].orientation.y,msg.pose[13].orientation.z);
Vector3d temp;
temp=R_left*vec_A_E;
//A=temp(2)+z_left;
a=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
temp=R_left*vec_B_F;
//B=temp(2)+z_left;
b=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
temp=R_left*vec_C_G;
//C=temp(2)+z_left;
c=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
temp=R_left*vec_D_H;
//D=temp(2)+z_left;
d=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
temp=R_right*vec_A_E;
//E=temp(2)+z_right;
e=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
temp=R_right*vec_B_F;
//F=temp(2)+z_right;
f=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
temp=R_right*vec_C_G;
//G=temp(2)+z_right;
g=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
temp=R_right*vec_D_H;
//H=temp(2)+z_right;
h=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);

 //ROS_INFO("z_left=%f  A=%d,B=%d,C=%d,D=%d\tz_right=%f  E=%d,F=%d,G=%d,H=%d",z_left,a,b,c,d,z_right,e,f,g,h);

//    ROS_INFO("x_left=%f,x_right=%f,y_left=%f,y_right=%fz_left=%f,z_right=%f",x_left,x_right,y_left,y_right,z_left,z_right);

}

ros::Publisher pub1  ;
ros::Publisher pub2  ;
ros::Publisher pub3  ;
ros::Publisher pub4  ;
ros::Publisher pub5  ;
ros::Publisher pub6  ;
ros::Publisher pub7  ;
ros::Publisher pub8  ;
ros::Publisher pub9  ;
ros::Publisher pub10 ;
ros::Publisher pub11 ;
ros::Publisher pub12 ;
ros::Publisher pub13 ;
ros::Publisher pub14 ;
ros::Publisher pub15 ;
ros::Publisher pub16 ;
ros::Publisher pub17 ;
ros::Publisher pub18 ;
ros::Publisher pub19 ;
ros::Publisher pub20 ;
ros::Publisher pub21 ;
ros::Publisher pub22 ;
ros::Publisher pub23 ;
ros::Publisher pub24 ;
ros::Publisher pub25 ;
ros::Publisher pub26 ;
ros::Publisher pub27 ;
ros::Publisher pub28 ;

//ros::Publisher pid1 ;

void  SendGazebo(QList<LinkM> links){
    if(links.count()<28){qDebug()<<"index err";return;}
    std_msgs::Float64 data;

    data.data=links[1].JointAngle;
    pub1.publish(data);
    data.data=links[2].JointAngle;
    pub2.publish(data);
    data.data=links[3].JointAngle;
    pub3.publish(data);
    data.data=links[4].JointAngle;
    pub4.publish(data);
    data.data=links[5].JointAngle;
    pub5.publish(data);
    data.data=links[6].JointAngle;
    pub6.publish(data);
    data.data=links[7].JointAngle;
    pub7.publish(data);
    data.data=links[8].JointAngle;
    pub8.publish(data);
    data.data=links[9].JointAngle;
    pub9.publish(data);
    data.data=links[10].JointAngle;
    pub10.publish(data);
    data.data=links[11].JointAngle;
    pub11.publish(data);
    data.data=links[12].JointAngle;
    pub12.publish(data);
    data.data=links[13].JointAngle;
    pub13.publish(data);
    data.data=links[14].JointAngle;
    pub14.publish(data);
    data.data=links[15].JointAngle;
    pub15.publish(data);
    data.data=links[16].JointAngle;
    pub16.publish(data);
    data.data=links[17].JointAngle;
    pub17.publish(data);
    data.data=links[18].JointAngle;
    pub18.publish(data);
    data.data=links[19].JointAngle;
    pub19.publish(data);
    data.data=links[20].JointAngle;
    pub20.publish(data);
    data.data=links[21].JointAngle;
    pub21.publish(data);
    data.data=links[22].JointAngle;
    pub22.publish(data);
    data.data=links[23].JointAngle;
    pub23.publish(data);
    data.data=links[24].JointAngle;
    pub24.publish(data);
    data.data=links[25].JointAngle;
    pub25.publish(data);
    data.data=links[26].JointAngle;
    pub26.publish(data);
    data.data=links[27].JointAngle;
    pub27.publish(data);
    data.data=links[28].JointAngle;
    pub28.publish(data);

}

int main(int argc, char **argv)
{
    vector<double> cntrl(13);
    QCgenerator QC;
    //check _timesteps
    QElapsedTimer timer;
    Robot SURENA;
    //QTime myTimer2;
    //myTimer2.start();
    TaskSpaceOffline SURENAOffilneTaskSpace;
    //ROS_INFO("time2 = %d",myTimer2.elapsed());
    QList<LinkM> links;
    MatrixXd PoseRoot;
    MatrixXd PoseRFoot;
    MatrixXd PoseLFoot;
    double dt;

    double k1;
    double k2;
    double k3;
    double k4;

    bool aState=false;
    bool bState=false;
    bool cState=false;
    bool dState=false;
    bool LeftFootLanded=false;

    double teta_motor_L=0;
    double teta_motor_R=0;
    double phi_motor_L=0;
    double phi_motor_R=0;

    int numberOfLeftFootSensorData=0;
    double footSensorSaturation=90;//if all sensors data are bigger than this amount, this means the foot is landed on the ground
    double footSensorthreshold=4;
    double hipRoll=0;
    double StartTime=0;
    double WalkTime=0;
    double RollTime=0;
    double  DurationOfStartPhase=6;
    double  DurationOfendPhase=6;
    //SURENAOffilneTaskSpace.GetAccVelPos();
    bool startPhase=true;
    bool endPhase=true;
    bool walk=true;
    PoseRoot.resize(6,1);
    PoseRFoot.resize(6,1);
    PoseLFoot.resize(6,1);

    //*******************This part of code is for initialization of joints of the robot for walking**********************************
    int count = 0;

    ros::init(argc, argv, "myNode");

    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);

    ros::Subscriber sub = nh.subscribe("/surena/bump_sensor_state", 1000, receiveFootSensor);

    pub1  = nh.advertise<std_msgs::Float64>("rrbot/joint1_position_controller/command",1000);
    pub2  = nh.advertise<std_msgs::Float64>("rrbot/joint2_position_controller/command",1000);
    pub3  = nh.advertise<std_msgs::Float64>("rrbot/joint3_position_controller/command",1000);
    pub4  = nh.advertise<std_msgs::Float64>("rrbot/joint4_position_controller/command",1000);
    pub5  = nh.advertise<std_msgs::Float64>("rrbot/joint5_position_controller/command",1000);
    pub6  = nh.advertise<std_msgs::Float64>("rrbot/joint6_position_controller/command",1000);
    pub7  = nh.advertise<std_msgs::Float64>("rrbot/joint7_position_controller/command",1000);
    pub8  = nh.advertise<std_msgs::Float64>("rrbot/joint8_position_controller/command",1000);
    pub9  = nh.advertise<std_msgs::Float64>("rrbot/joint9_position_controller/command",1000);
    pub10 = nh.advertise<std_msgs::Float64>("rrbot/joint10_position_controller/command",1000);
    pub11 = nh.advertise<std_msgs::Float64>("rrbot/joint11_position_controller/command",1000);
    pub12 = nh.advertise<std_msgs::Float64>("rrbot/joint12_position_controller/command",1000);
    pub13 = nh.advertise<std_msgs::Float64>("rrbot/joint13_position_controller/command",1000);
    pub14 = nh.advertise<std_msgs::Float64>("rrbot/joint14_position_controller/command",1000);
    pub15 = nh.advertise<std_msgs::Float64>("rrbot/joint15_position_controller/command",1000);
    pub16 = nh.advertise<std_msgs::Float64>("rrbot/joint16_position_controller/command",1000);
    pub17 = nh.advertise<std_msgs::Float64>("rrbot/joint17_position_controller/command",1000);
    pub18 = nh.advertise<std_msgs::Float64>("rrbot/joint18_position_controller/command",1000);
    pub19 = nh.advertise<std_msgs::Float64>("rrbot/joint19_position_controller/command",1000);
    pub20 = nh.advertise<std_msgs::Float64>("rrbot/joint20_position_controller/command",1000);
    pub21 = nh.advertise<std_msgs::Float64>("rrbot/joint21_position_controller/command",1000);
    pub22 = nh.advertise<std_msgs::Float64>("rrbot/joint22_position_controller/command",1000);
    pub23 = nh.advertise<std_msgs::Float64>("rrbot/joint23_position_controller/command",1000);
    pub24 = nh.advertise<std_msgs::Float64>("rrbot/joint24_position_controller/command",1000);
    pub25 = nh.advertise<std_msgs::Float64>("rrbot/joint25_position_controller/command",1000);
    pub26 = nh.advertise<std_msgs::Float64>("rrbot/joint26_position_controller/command",1000);
    pub27 = nh.advertise<std_msgs::Float64>("rrbot/joint27_position_controller/command",1000);
    pub28 = nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/command",1000);

    //pid1= nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/pid/parameter_updates",1000);

    ros::Rate loop_rate(200);
    std_msgs::Int32MultiArray msg;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);
    k1=0.00009;
    k2=0.00009;
    k3=0.00001;
    k4=0.00001;

    while (ros::ok())
    {
        //QTime myTimer;
        //myTimer.start();
        //-------------for detecting the full contact of foot with ground-------------//
        if ((a)>=footSensorSaturation && (b)>=footSensorSaturation && (c)>=footSensorSaturation && (d)>=footSensorSaturation){
            qDebug("swing foot landing is successful");
            aState=true;
            bState=true;
            cState=true;
            dState=true;
          //  LeftFootLanded=true;//this variable is used for flag up when the left foot have a full contact with the ground

            //do nothing all sensors are on the ground
            teta_motor_L=teta_motor_L;
            phi_motor_L=phi_motor_L;

            teta_motor_R=teta_motor_R;
            phi_motor_R=phi_motor_R;
            //exit(0);//will finish the code but during walking is not true
        }

        else {//---------when the four sensors are not saturated-->>>>during landing------//

            LeftFootLanded=false;
            if ((a)>=footSensorthreshold ){aState=true;}else {aState=false;}
            if ((b)>=footSensorthreshold ){bState=true;}else {bState=false;}
            if ((c)>=footSensorthreshold ){cState=true;}else {cState=false;}
            if ((d)>=footSensorthreshold ){dState=true;}else {dState=false;}

            //-----------------Pitch left ankle motor control---------------//
            if (abs(b-a)>=abs(c-d)) {
                if (abs(a-b)<100) {
                    teta_motor_L=1*teta_motor_L+k1*(a-b);
                }
                else {
                    teta_motor_L=teta_motor_L;
                }
            }
            else {
                if (abs(d-c)<100) {
                    teta_motor_L=1*teta_motor_L+k1*(d-c);
                }
                else {
                    teta_motor_L=teta_motor_L;
                }
            }

            //----------------Roll left ankle motor control---------------//
            if (abs(c-b)>=abs(d-a)) {
                if (abs(c-b)<100) {
                    phi_motor_L=1*phi_motor_L+k2*(c-b);
                }
                else {
                    phi_motor_L=phi_motor_L;
                }
            }
            else {
                if (abs(a-d)<100) {
                    phi_motor_L=1*phi_motor_L+k2*(d-a);
                }
                else {
                    phi_motor_L=phi_motor_L;
                }
            }
        }

        //------------------------saturation of ankle motors----------------------------//
        if ((abs(phi_motor_L))>0.9) {
            phi_motor_L=0.9;
        }
        if ((abs(teta_motor_L))>0.9) {
            teta_motor_L=0.9;
        }
        if ((abs(phi_motor_R))>0.9) {
            phi_motor_L=0.9;
        }
        if ((abs(teta_motor_R))>0.9) {
            teta_motor_R=0.9;
        }

        // ROS_INFO("I heard data of sensors : [%f %f %f %f]",a,b,c,d);

        //-----------------------------------------------------------------------------------------------------//
        //-----------------start phase--initializing the height of pelvis for walking--------------------------//
        //-----------------------------------------------------------------------------------------------------//
        if (startPhase==true && StartTime<=DurationOfStartPhase) {
            MinimumJerkInterpolation Coef;
            MatrixXd ZPosition(1,2);
            ZPosition<<0.95100,0.8300;
            MatrixXd ZVelocity(1,2);
            ZVelocity<<0.000,0.000;
            MatrixXd ZAcceleration(1,2);
            ZAcceleration<<0.000,0.000;

            MatrixXd Time(1,2);
            Time<<0,DurationOfStartPhase;
            MatrixXd CoefZStart =Coef.Coefficient(Time,ZPosition,ZVelocity,ZAcceleration);

            double zStart=0;
            double yStart=0;
            double xStart=0;
            StartTime=StartTime+SURENAOffilneTaskSpace._timeStep;

            MatrixXd outputZStart= SURENAOffilneTaskSpace.GetAccVelPos(CoefZStart,StartTime,0,5);
            zStart=outputZStart(0,0);

            PoseRoot<<xStart,yStart,zStart,0,0,0;

            PoseRFoot<<0,
                    -0.11500,
                    0.112000,
                    0,
                    0,
                    0;

            PoseLFoot<<0,
                    0.11500,
                    0.11200,
                    0,
                    0,
                    0;

            SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
            SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);
        }

        //-----------------------------------------------------------------------------------------------------//
        //------------------------------- main loop of cyclic walking -----------------------------------------//
        //-----------------------------------------------------------------------------------------------------//

        if (StartTime>DurationOfStartPhase && StartTime<(DurationOfStartPhase+SURENAOffilneTaskSpace.MotionTime)){

            double m1;
            double m2;
            double m3;
            double m4;
            double m5;
            double m6;
            double m7;
            double m8;

            StartTime=StartTime+SURENAOffilneTaskSpace._timeStep;
            //qDebug()<<StartTime;
            MatrixXd P;
            if(walk==true){
                MatrixXd m=SURENAOffilneTaskSpace.AnkleTrajectory(SURENAOffilneTaskSpace.globalTime);
                m1=m(0,0);
                m2=m(1,0);
                m3=m(2,0);
                m4=m(3,0);
                m5=m(4,0);
                m6=m(5,0);
                m7=m(6,0);
                m8=m(7,0);

                P=SURENAOffilneTaskSpace.PelvisTrajectory (SURENAOffilneTaskSpace.globalTime);
                SURENAOffilneTaskSpace.globalTime=SURENAOffilneTaskSpace.globalTime+SURENAOffilneTaskSpace._timeStep;

                if (round(SURENAOffilneTaskSpace.globalTime)<=round(SURENAOffilneTaskSpace.MotionTime)){

                    //--------------------- hip roll modification is commented and is not active --------------------------//
                    //--------------------- hip roll modification is commented and is not active --------------------------//
//                    if (SURENAOffilneTaskSpace.DoubleSupport!=true) {
//                        //For modifying the angle of roll during single support
//                        RollTime=RollTime+SURENAOffilneTaskSpace._timeStep;
//                        MinimumJerkInterpolation Coef;
//                        MatrixXd RollAngle(1,3);
//                        RollAngle<<0,0.0,0;
//                        MatrixXd RollAngleVelocity(1,3);
//                        RollAngleVelocity<<0.000,INFINITY,0.000;
//                        MatrixXd RollAngleAcceleration(1,3);
//                        RollAngleAcceleration<<0,INFINITY,0;
//                        MatrixXd Time22(1,3);
//                        Time22<<0,(SURENAOffilneTaskSpace.TSS/2),SURENAOffilneTaskSpace.TSS;
//                        MatrixXd CoefRoll =Coef.Coefficient(Time22,RollAngle,RollAngleVelocity,RollAngleAcceleration);
//                        //StartTime=StartTime+SURENAOffilneTaskSpace._timeStep;
//                        MatrixXd outputRollAngle;
//                        if (RollTime<=(SURENAOffilneTaskSpace.TSS/2)) {
//                            outputRollAngle= SURENAOffilneTaskSpace.GetAccVelPos(CoefRoll.row(0),RollTime,0,5);
//                            hipRoll=outputRollAngle(0,0);
//                        }
//                        else {
//                            outputRollAngle =SURENAOffilneTaskSpace.GetAccVelPos(CoefRoll.row(1),RollTime,SURENAOffilneTaskSpace.TSS/2,5);
//                            hipRoll=outputRollAngle(0,0);
//                        }
//                    }
//                    else {
//                        hipRoll=0;
//                        RollTime=0;
//                    }

                    PoseRoot<<P(0,0),
                            P(1,0),
                            P(2,0),
                            0,
                            0,
                            0;

                    PoseRFoot<<m5,
                            m6,
                            m7,
                            0,
                            -1*m8*(M_PI/180),
                            0;

                    PoseLFoot<<m1,
                            m2,
                            m3,
                            0,
                            -1*m4*(M_PI/180),
                            0;

                    //// hip roll modification
                    //                    if (SURENAOffilneTaskSpace.LeftSupport==true && SURENAOffilneTaskSpace.HipRollModification==true){
                    //                        SURENA.doIKhipRollModify("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot,-1*hipRoll);
                    //                        SURENA.doIKhipRollModify("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot,0);
                    //                    }

                    //                    else if (SURENAOffilneTaskSpace.HipRollModification==true && SURENAOffilneTaskSpace.LeftSupport!=true )  {
                    //                        SURENA.doIKhipRollModify("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot,0);
                    //                        SURENA.doIKhipRollModify("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot,0*hipRoll);
                    //                    }

                    SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
                    SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);
                }
            }
        }

        //-----------------------------------------------------------------------------------------------------//
        //------------------- end phase-- finializing height of pelvis to home position -----------------------//
        //-----------------------------------------------------------------------------------------------------//

        if (endPhase==true &&  StartTime>=(DurationOfStartPhase+SURENAOffilneTaskSpace.MotionTime) && StartTime<=DurationOfendPhase+DurationOfStartPhase+SURENAOffilneTaskSpace.MotionTime) {
            MinimumJerkInterpolation Coef;
            MatrixXd ZPosition(1,2);
            ZPosition<<0.830,0.95100;
            MatrixXd ZVelocity(1,2);
            ZVelocity<<0.000,0.000;
            MatrixXd ZAcceleration(1,2);
            ZAcceleration<<0.000,0.000;

            MatrixXd Time(1,2);
            Time<<DurationOfStartPhase+SURENAOffilneTaskSpace.MotionTime,DurationOfStartPhase+SURENAOffilneTaskSpace.MotionTime+DurationOfendPhase;
            MatrixXd CoefZStart =Coef.Coefficient(Time,ZPosition,ZVelocity,ZAcceleration);

            double zStart=0;
            double yStart=0;
            double xStart=0;
            StartTime=StartTime+SURENAOffilneTaskSpace._timeStep;

            MatrixXd outputZStart= SURENAOffilneTaskSpace.GetAccVelPos(CoefZStart,StartTime,DurationOfStartPhase+SURENAOffilneTaskSpace.MotionTime,5);
            zStart=outputZStart(0,0);

            PoseRoot<<xStart,yStart,zStart,0,0,0;

            PoseRFoot<<0,
                    -0.11500,
                    0.112000,
                    0,
                    0,
                    0;

            PoseLFoot<<0,
                    0.11500,
                    0.11200,
                    0,
                    0,
                    0;

            SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
            SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);
        }

        links = SURENA.GetLinks();
        msg.data.clear();

        if (LeftFootLanded==true) {//else if is for situation that foot landed successfully on the ground
            cntrl[0]=0.0;
            cntrl[1]=cntrl[1];
            cntrl[2]=cntrl[2];
            cntrl[3]=cntrl[3];
            cntrl[4]=cntrl[4];
            cntrl[5]=cntrl[5];
            cntrl[6]=cntrl[6];//roll
            cntrl[7]=cntrl[7];
            cntrl[8]=cntrl[8];
            cntrl[9]=cntrl[9];
            cntrl[10]=cntrl[10];
            cntrl[11]=cntrl[11];
            cntrl[12]=cntrl[12];

            walk=false;
            endPhase=false;
            startPhase=false;
        }

        else {//else  is for situation that sensor is contacting and adapting ground
            cntrl[0]=0.0;
            cntrl[1]=links[1].JointAngle;
            cntrl[2]=links[2].JointAngle;
            cntrl[3]=links[3].JointAngle;
            cntrl[4]=links[4].JointAngle;
            cntrl[5]=links[5].JointAngle;//+teta_motor_R;//pitch
            cntrl[6]=links[6].JointAngle;//roll
            cntrl[7]=links[7].JointAngle;
            cntrl[8]=links[8].JointAngle;
            cntrl[9]=links[9].JointAngle;
            cntrl[10]=links[10].JointAngle;
            cntrl[11]=+0*teta_motor_L+links[11].JointAngle;
            cntrl[12]=+0*phi_motor_L+links[12].JointAngle;
        }

        vector<int> qref(12);
        qref=QC.ctrldata2qc(cntrl);

        for(int  i = 0;i < 12;i++)
        {
           // msg.data.push_back(qref[i]);
            msg.data.push_back(0);
        }

        SendGazebo(links);
        chatter_pub.publish(msg);
        //  ROS_INFO("t={%d} c={%d}",timer.elapsed(),count);
//        ROS_INFO("time = %d", myTimer.elapsed());

        ros::spinOnce();
        loop_rate.sleep();
        //ROS_INFO("time = %d", myTimer.elapsed());
        ++count;
    }

    return 0;
}


#ifndef DART_NEURAL_MAPPED_BACKPROP_SNAPSHOT_HPP_
#define DART_NEURAL_MAPPED_BACKPROP_SNAPSHOT_HPP_

#include <memory>
#include <optional>
#include <unordered_map>
#include <vector>

#include <Eigen/Dense>

#include "dart/neural/BackpropSnapshot.hpp"
#include "dart/neural/Mapping.hpp"
#include "dart/performance/PerformanceLog.hpp"

namespace dart {

using namespace performance;

namespace simulation {
class World;
}

namespace neural {

class BackpropSnapshot;

// Before we take a step, we need to map "out" of the mapped space and back into
// world space. Then we can take our step in world space, and map back "in" to
// the mapped space.
struct PreStepMapping
{
  Eigen::VectorXs pos;
  Eigen::VectorXs vel;
  Eigen::VectorXs force;
  Eigen::VectorXs mass;

  PreStepMapping(
      std::shared_ptr<simulation::World> world,
      std::shared_ptr<Mapping> mapping)
  {
    pos = mapping->getPositions(world);
    vel = mapping->getVelocities(world);
    force = mapping->getControlForces(world);
    mass = mapping->getMasses(world);
  }

  PreStepMapping(){};
};

// After we take a step, we need to map "in" to the mapped space, from world
// space where we took the step.
struct PostStepMapping
{
  Eigen::VectorXs pos;
  Eigen::MatrixXs posInJacWrtPos;
  Eigen::MatrixXs posInJacWrtVel;

  Eigen::VectorXs vel;
  Eigen::MatrixXs velInJacWrtPos;
  Eigen::MatrixXs velInJacWrtVel;

  PostStepMapping(
      std::shared_ptr<simulation::World> world,
      std::shared_ptr<Mapping> mapping)
  {
    pos = mapping->getPositions(world);
    posInJacWrtPos = mapping->getRealPosToMappedPosJac(world);
    posInJacWrtVel = mapping->getRealVelToMappedPosJac(world);

    vel = mapping->getVelocities(world);
    velInJacWrtPos = mapping->getRealPosToMappedVelJac(world);
    velInJacWrtVel = mapping->getRealVelToMappedVelJac(world);
  }

  PostStepMapping(){};
};

class MappedBackpropSnapshot
{
public:
  MappedBackpropSnapshot(
      std::shared_ptr<BackpropSnapshot> backpropSnapshot,
      std::unordered_map<std::string, std::shared_ptr<Mapping>> mappings,
      std::unordered_map<std::string, PreStepMapping> preStepMappings,
      std::unordered_map<std::string, PostStepMapping> postStepMappings);

  const std::vector<std::string>& getMappings();

  const Eigen::MatrixXs& getPosPosJacobian(
      std::shared_ptr<simulation::World> world,
      PerformanceLog* perfLog = nullptr);
  const Eigen::MatrixXs& getPosVelJacobian(
      std::shared_ptr<simulation::World> world,
      PerformanceLog* perfLog = nullptr);
  const Eigen::MatrixXs& getVelPosJacobian(
      std::shared_ptr<simulation::World> world,
      PerformanceLog* perfLog = nullptr);
  const Eigen::MatrixXs& getVelVelJacobian(
      std::shared_ptr<simulation::World> world,
      PerformanceLog* perfLog = nullptr);
  const Eigen::MatrixXs& getControlForceVelJacobian(
      std::shared_ptr<simulation::World> world,
      PerformanceLog* perfLog = nullptr);
  const Eigen::MatrixXs& getMassVelJacobian(
      std::shared_ptr<simulation::World> world,
      PerformanceLog* perfLog = nullptr);

  Eigen::MatrixXs getPosMappedPosJacobian(
      std::shared_ptr<simulation::World> world,
      const std::string& mapAfter,
      PerformanceLog* perfLog = nullptr);
  Eigen::MatrixXs getPosMappedVelJacobian(
      std::shared_ptr<simulation::World> world,
      const std::string& mapAfter,
      PerformanceLog* perfLog = nullptr);
  Eigen::MatrixXs getVelMappedPosJacobian(
      std::shared_ptr<simulation::World> world,
      const std::string& mapAfter,
      PerformanceLog* perfLog = nullptr);
  Eigen::MatrixXs getVelMappedVelJacobian(
      std::shared_ptr<simulation::World> world,
      const std::string& mapAfter,
      PerformanceLog* perfLog = nullptr);
  Eigen::MatrixXs getControlForceMappedVelJacobian(
      std::shared_ptr<simulation::World> world,
      const std::string& mapAfter,
      PerformanceLog* perfLog = nullptr);
  Eigen::MatrixXs getMassMappedVelJacobian(
      std::shared_ptr<simulation::World> world,
      const std::string& mapAfter,
      PerformanceLog* perfLog = nullptr);

  /// This computes the implicit backprop without forming intermediate
  /// Jacobians. It takes a LossGradient with the position and velocity vectors
  /// filled it, though the loss with respect to torque is ignored and can be
  /// null. It returns a LossGradient with all three values filled in, position,
  /// velocity, and torque.
  void backprop(
      simulation::WorldPtr world,
      LossGradient& thisTimestepLoss,
      const std::unordered_map<std::string, LossGradient> nextTimestepLosses,
      PerformanceLog* perfLog = nullptr,
      bool exploreAlternateStrategies = false);

  /// Returns a concatenated vector of all the Skeletons' position()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, BEFORE the timestep.
  const Eigen::VectorXs& getPreStepPosition(
      const std::string& mapping = "identity");

  /// Returns a concatenated vector of all the Skeletons' velocity()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, BEFORE the timestep.
  const Eigen::VectorXs& getPreStepVelocity(
      const std::string& mapping = "identity");

  /// Returns a concatenated vector of all the joint torques that were applied
  /// during the forward pass, BEFORE the timestep.
  const Eigen::VectorXs& getPreStepTorques(
      const std::string& mapping = "identity");

  /// Returns the LCP's cached solution from before the step
  const Eigen::VectorXs& getPreStepLCPCache();

  /// Returns a concatenated vector of all the Skeletons' position()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, AFTER the timestep.
  const Eigen::VectorXs& getPostStepPosition(const std::string& mapping);

  /// Returns a concatenated vector of all the Skeletons' velocity()'s in the
  /// World, in order in which the Skeletons appear in the World's
  /// getSkeleton(i) returns them, AFTER the timestep.
  const Eigen::VectorXs& getPostStepVelocity(const std::string& mapping);

  /// Returns the underlying BackpropSnapshot, without the mappings
  std::shared_ptr<BackpropSnapshot> getUnderlyingSnapshot();

  /// This computes and returns the whole vel-vel jacobian by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceVelVelJacobian(
      simulation::WorldPtr world,
      const std::string& mapAfter = "identity",
      bool useRidders = true);

  /// This computes and returns the whole vel-vel jacobian by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersVelVelJacobian(
      simulation::WorldPtr world, const std::string& mapAfter = "identity");

  /// This computes and returns the whole pos-C(pos,vel) jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferencePosVelJacobian(
      simulation::WorldPtr world,
      const std::string& mapAfter = "identity",
      bool useRidders = true);

  /// This computes and returns the whole pos-C(pos,vel) jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersPosVelJacobian(
      simulation::WorldPtr world, const std::string& mapAfter = "identity");

  /// This computes and returns the whole force-vel jacobian by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceForceVelJacobian(
      simulation::WorldPtr world,
      const std::string& mapAfter = "identity",
      bool useRidders = true);

  /// This computes and returns the whole force-vel jacobian by finite
  /// differences. This is SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersForceVelJacobian(
      simulation::WorldPtr world, const std::string& mapAfter = "identity");

  /// This computes a finite differenced Jacobian for pos_t->mapped_pos_{t+1}
  Eigen::MatrixXs finiteDifferencePosPosJacobian(
      std::shared_ptr<simulation::World> world,
      const std::string& mapAfter = "identity",
      std::size_t subdivisions = 20,
      bool useRidders = true);

  /// This computes a finite differenced Jacobian for pos_t->mapped_pos_{t+1}
  Eigen::MatrixXs finiteDifferenceRiddersPosPosJacobian(
      std::shared_ptr<simulation::World> world,
      const std::string& mapAfter = "identity",
      std::size_t subdivisions = 20);

  /// This computes and returns the whole vel-pos jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceVelPosJacobian(
      simulation::WorldPtr world,
      const std::string& mapAfter = "identity",
      std::size_t subdivisions = 20,
      bool useRidders = true);

  /// This computes and returns the whole vel-pos jacobian by finite
  /// differences. This is SUPER SUPER SLOW, and is only here for testing.
  Eigen::MatrixXs finiteDifferenceRiddersVelPosJacobian(
      simulation::WorldPtr world,
      const std::string& mapAfter = "identity",
      std::size_t subdivisions = 20);

protected:
  std::shared_ptr<BackpropSnapshot> mBackpropSnapshot;
  std::vector<std::string> mMappingsSet;
  std::unordered_map<std::string, std::shared_ptr<Mapping>> mMappings;
  std::unordered_map<std::string, PreStepMapping> mPreStepMappings;
  std::unordered_map<std::string, PostStepMapping> mPostStepMappings;
};

using MappedBackpropSnapshotPtr = std::shared_ptr<MappedBackpropSnapshot>;

} // namespace neural
} // namespace dart

#endif


/*!
 * Title ---- km/RsaKey.hpp
 * Author --- Giacomo Trudu aka `Wicker25` - wicker25[at]gmail[dot]com
 *
 * Copyright (C) 2017 by Giacomo Trudu.
 * All rights reserved.
 *
 * This file is part of KeyMaker software.
 */

#ifndef __KM_RSA_KEY_HPP__
#define __KM_RSA_KEY_HPP__

#include <km.hpp>
#include <km/Exception.hpp>

#include <memory>

#include <boost/filesystem.hpp>

#include <openssl/bn.h>
#include <openssl/rsa.h>
#include <openssl/pem.h>
#include <openssl/err.h>

namespace km { // Begin main namespace

class Encrypter;

/*!
 * The RSA key.
 */
class RsaKey
{
    friend class Encrypter;

public:

    /*!
     * Constructor method.
     */
    RsaKey();

    /*!
     * Destructor method.
     */
	virtual ~RsaKey();

protected:

    /*!
     * The RSA data.
     */
    std::shared_ptr<RSA> mRsa;
};

} // End of main namespace

#endif /* __KM_RSA_KEY_HPP__ */

// Include inline methods
#include <km/RsaKey-inl.hpp>


// Copyright © 2017-2020 Trust Wallet.
//
// This file is part of Trust. The full Trust copyright notice, including
// terms governing use, modification, and redistribution, is contained in the
// file LICENSE at the root of the source code distribution tree.

#include "ParamNumber.h"

#include <Data.h>
#include <uint256.h>
#include <HexCoding.h>

#include <boost/lexical_cast.hpp>

#include <string>
#include <cassert>

using namespace TW;
using namespace TW::Ethereum::ABI;

bool ParamUInt256::setUInt256FromValueJson(uint256_t& dest, const std::string& value) {
    // try hex string or number
    if (value.length() >= 3 && value.substr(0, 2) == "0x") {
        dest = load(parse_hex(value));
        return true;
    }
    return boost::conversion::detail::try_lexical_convert(value, dest);
}

bool ParamInt256::setInt256FromValueJson(int256_t& dest, const std::string& value) {
    // try hex string or number
    if (value.length() >= 3 && value.substr(0, 2) == "0x") {
        dest = ValueEncoder::int256FromUint256(load(parse_hex(value)));
        return true;
    }
    return boost::conversion::detail::try_lexical_convert(value, dest);
}

bool ParamBool::setValueJson(const std::string& value) {
    if (value == "true" || value == "1") { setVal(true); return true; }
    if (value == "false" || value == "0") { setVal(false); return true; }
    return false;
}

bool ParamUInt8::setValueJson(const std::string& value) {
    uint16_t val;
    if (!boost::conversion::detail::try_lexical_convert(value, val)) { return false; }
    setVal(static_cast<uint8_t>(val));
    return true;
}

bool ParamInt8::setValueJson(const std::string& value) {
    int16_t val;
    if (!boost::conversion::detail::try_lexical_convert(value, val)) { return false; }
    setVal(static_cast<int8_t>(val));
    return true;
}

void ParamUIntN::setVal(uint256_t val) {
    // mask it to the given bits
    _val = val & _mask;
}

bool ParamUIntN::decode(const Data& encoded, size_t& offset_inout) {
    uint256_t temp;
    auto res = decodeNumber(encoded, temp, offset_inout);
    setVal(temp);
    return res;
}

void ParamUIntN::init() {
    _mask = maskForBits(bits);
}

uint256_t ParamUIntN::maskForBits(size_t bits) {
    assert(bits >= 8 && bits <= 256 && (bits % 8) == 0);
    // exclude predefined sizes
    assert(bits != 8 && bits != 16 && bits != 32 && bits != 64 && bits != 256);
    return (uint256_t(1) << bits) - 1;
}

void ParamIntN::setVal(int256_t val) {
    // mask it to the given bits
    if (val < 0) {
        _val = ValueEncoder::int256FromUint256(~((~((uint256_t)val)) & _mask));
    } else {
        _val = ValueEncoder::int256FromUint256(((uint256_t)val) & _mask);
    }
}

bool ParamIntN::decodeNumber(const Data& encoded, int256_t& decoded, size_t& offset_inout) {
    uint256_t valU;
    auto res = ABI::decode(encoded, valU, offset_inout);
    decoded = ValueEncoder::int256FromUint256(valU);
    return res;
}

bool ParamIntN::decode(const Data& encoded, size_t& offset_inout) {
    int256_t temp;
    auto res = decodeNumber(encoded, temp, offset_inout);
    setVal(temp);
    return res;
}

void ParamIntN::init()
{
    _mask = ParamUIntN::maskForBits(bits);
}


/*  _______________________________________________________________________

    DAKOTA: Design Analysis Kit for Optimization and Terascale Applications
    Copyright 2014 Sandia Corporation.
    This software is distributed under the GNU Lesser General Public License.
    For more information, see the README file in the top Dakota directory.
    _______________________________________________________________________ */

#ifndef DATA_TYPES_H
#define DATA_TYPES_H

#include "dakota_system_defs.hpp"
#include "Teuchos_SerialDenseVector.hpp"
#include <boost/multi_array.hpp>
#include "boost/dynamic_bitset.hpp"
#include <list>

namespace Dakota {

// avoid problems with circular dependencies by using fwd declarations
class Iterator;
class Model;
class Variables;
class Interface;
class Response;
class ActiveSet;
class ParamResponsePair;
class ParallelLevel;
class ParallelConfiguration;
class DiscrepancyCorrection;

// -----------------------------------
// Aliases for fundamental data types:
// -----------------------------------
typedef double Real;
typedef std::string String;

// -----------------------------------
// Numerical arrays (serial/parallel):
// -----------------------------------

typedef Teuchos::SerialDenseVector<int, Real>    RealVector;
typedef Teuchos::SerialDenseMatrix<int, Real>    RealMatrix;
typedef Teuchos::SerialSymDenseMatrix<int, Real> RealSymMatrix;
typedef Teuchos::SerialDenseVector<int, int>     IntVector;
typedef Teuchos::SerialDenseMatrix<int, int>     IntMatrix;

//typedef Tpetra::CrsMatrix<int, Real>             RealSparseMatrix
//typedef Tpetra::Vector<int, Real>                RealParallelVector
//typedef Tpetra::MultiVector<int, Real>           RealParallelMatrix

// ---------------------------------------
// Admin/bookkeeping arrays (serial only):
// ---------------------------------------
typedef std::deque<bool> BoolDeque; // See Meyers' Effective STL #18
typedef boost::dynamic_bitset<unsigned long> BitArray;

typedef std::vector<BoolDeque>      BoolDequeArray;
typedef std::vector<Real>           RealArray;
typedef std::vector<RealArray>      Real2DArray;
typedef std::vector<int>            IntArray;
typedef std::vector<IntArray>       Int2DArray;
typedef std::vector<short>          ShortArray;
typedef std::vector<unsigned short> UShortArray;
typedef std::vector<UShortArray>    UShort2DArray;
typedef std::vector<UShort2DArray>  UShort3DArray;
typedef std::vector<size_t>         SizetArray;
typedef std::vector<SizetArray>     Sizet2DArray;
typedef std::vector<Sizet2DArray>   Sizet3DArray;
typedef std::vector<String>         StringArray;
typedef std::vector<StringArray>    String2DArray;

typedef boost::multi_array_types::index_range       idx_range;
typedef boost::multi_array<String, 1>               StringMultiArray;
typedef boost::multi_array<String, 2>               StringMulti2DArray;
typedef StringMultiArray::array_view<1>::type       StringMultiArrayView;
typedef StringMultiArray::const_array_view<1>::type StringMultiArrayConstView;
typedef boost::multi_array<unsigned short, 1>       UShortMultiArray;
typedef UShortMultiArray::array_view<1>::type       UShortMultiArrayView;
typedef UShortMultiArray::const_array_view<1>::type UShortMultiArrayConstView;
typedef boost::multi_array<size_t, 1>               SizetMultiArray;
typedef SizetMultiArray::array_view<1>::type        SizetMultiArrayView;
typedef SizetMultiArray::const_array_view<1>::type  SizetMultiArrayConstView;
typedef boost::multi_array<Real, 1>                 RealMultiArray;
typedef boost::multi_array<Real, 2>                 RealMulti2DArray;
typedef boost::multi_array<Real, 3>                 RealMulti3DArray;

typedef std::vector<RealVector>        RealVectorArray;
typedef std::vector<RealVectorArray>   RealVector2DArray;
typedef std::vector<RealMatrix>        RealMatrixArray;
typedef std::vector<RealSymMatrix>     RealSymMatrixArray;
typedef std::vector<IntVector>         IntVectorArray;
typedef std::vector<Variables>         VariablesArray;
typedef std::vector<Response>          ResponseArray;
typedef std::vector<ParamResponsePair> PRPArray;
typedef std::vector<PRPArray>          PRP2DArray;
typedef std::vector<Model>             ModelArray;
typedef std::vector<Iterator>          IteratorArray;
typedef std::vector<RealMultiArray>    BoostMAArray;
typedef std::vector<RealMulti2DArray>  BoostMA2DArray;
typedef std::vector<RealMulti3DArray>  BoostMA3DArray;

typedef std::list<bool>                BoolList;
typedef std::list<int>                 IntList;
typedef std::list<size_t>              SizetList;
typedef std::list<Real>                RealList;
typedef std::list<RealVector>          RealVectorList;
typedef std::list<String>              StringList;
typedef std::list<Variables>           VariablesList;
typedef std::list<Interface>           InterfaceList;
typedef std::list<Response>            ResponseList;
typedef std::list<ParamResponsePair>   PRPList;
typedef std::list<Model>               ModelList;
typedef std::list<Iterator>            IteratorList;
//typedef std::list<ParallelLevel>         ParLevList;
//typedef std::list<ParallelConfiguration> ParConfigList;

typedef std::pair<int, int>            IntIntPair;
typedef std::pair<size_t, size_t>      SizetSizetPair;
typedef std::pair<SizetSizetPair, SizetSizetPair> SizetSizet2DPair;
typedef std::pair<size_t, int>         SizetIntPair;
typedef std::pair<int, size_t>         IntSizetPair;
typedef std::pair<int, String>         IntStringPair;
typedef std::pair<Real, Real>          RealRealPair;
typedef std::pair<int, Response>       IntResponsePair;
typedef std::set<Real>                 RealSet;
typedef std::set<int>                  IntSet;
typedef std::set<String>               StringSet;
typedef std::set<unsigned short>       UShortSet;
typedef std::set<size_t>               SizetSet;
typedef std::set<RealRealPair>         RealRealPairSet;
typedef std::vector<RealSet>           RealSetArray;
typedef std::vector<IntSet>            IntSetArray;
typedef std::vector<StringSet>         StringSetArray;
typedef std::vector<UShortSet>         UShortSetArray;
typedef std::map<int, int>             IntIntMap;
typedef std::map<int, short>           IntShortMap;
typedef std::map<int, Real>            IntRealMap;
typedef std::map<Real, Real>           RealRealMap;
typedef std::map<String, Real>         StringRealMap;
typedef std::multimap<Real, int>       RealIntMultiMap;
typedef std::vector<RealRealPair>      RealRealPairArray;
typedef std::vector<IntRealMap>        IntRealMapArray;
typedef std::vector<RealRealMap>       RealRealMapArray;
typedef std::vector<StringRealMap>     StringRealMapArray;
typedef std::map<int, RealVector>      IntRealVectorMap;
typedef std::map<int, RealMatrix>      IntRealMatrixMap;
typedef std::map<int, ActiveSet>       IntActiveSetMap;
typedef std::map<int, Variables>       IntVariablesMap;
typedef std::map<int, Response>        IntResponseMap;
typedef std::map<IntArray, size_t>     IntArraySizetMap;
typedef std::map<IntIntPair, Real>     IntIntPairRealMap;
typedef std::map<IntIntPair, RealMatrix> IntIntPairRealMatrixMap;
typedef std::map<RealRealPair, Real>     RealRealPairRealMap;
typedef std::vector<IntIntPairRealMap>   IntIntPairRealMapArray;
typedef std::vector<RealRealPairRealMap> RealRealPairRealMapArray;
typedef std::multimap<RealRealPair, ParamResponsePair> RealPairPRPMultiMap;
//typedef std::multimap<Real, ParamResponsePair> RealPRPMultiMap;
typedef std::map<SizetSizet2DPair, DiscrepancyCorrection> DiscrepCorrMap;

// ---------
// Iterators
// ---------
typedef IntList::iterator                          ILIter;
typedef IntList::const_iterator                    ILCIter;
typedef SizetList::iterator                        StLIter;
typedef SizetList::const_iterator                  StLCIter;
typedef RealList::iterator                         RLIter;
typedef RealList::const_iterator                   RLCIter;
typedef RealVectorList::iterator                   RVLIter;
typedef RealVectorList::const_iterator             RVLCIter;
typedef StringList::iterator                       StringLIter;
typedef StringList::const_iterator                 StringLCIter;
typedef VariablesList::iterator                    VarsLIter;
typedef InterfaceList::iterator                    InterfLIter;
typedef ResponseList::iterator                     RespLIter;
typedef PRPList::iterator                          PRPLIter;
typedef ModelList::iterator                        ModelLIter;
typedef ModelList::reverse_iterator                ModelLRevIter;
typedef IteratorList::iterator                     IterLIter;
typedef std::list<ParallelLevel>::iterator         ParLevLIter;
typedef std::list<ParallelConfiguration>::iterator ParConfigLIter;

typedef IntSet::iterator                       ISIter;
typedef IntSet::const_iterator                 ISCIter;
typedef StringSet::iterator                    SSIter;
typedef StringSet::const_iterator              SSCIter;
typedef RealSet::iterator                      RSIter;
typedef RealSet::const_iterator                RSCIter;
typedef IntIntMap::iterator                    IntIntMIter;
typedef IntIntMap::const_iterator              IntIntMCIter;
typedef IntShortMap::iterator                  IntShMIter;
typedef IntShortMap::const_iterator            IntShMCIter;
typedef IntRealMap::iterator                   IRMIter;
typedef IntRealMap::const_iterator             IRMCIter;
typedef StringRealMap::iterator                SRMIter;
typedef StringRealMap::const_iterator          SRMCIter;
typedef RealRealMap::iterator                  RRMIter;
typedef RealRealMap::const_iterator            RRMCIter;
typedef IntIntPairRealMap::iterator            IIPRMIter;
typedef IntIntPairRealMap::const_iterator      IIPRMCIter;
typedef RealRealPairSet::iterator              RRPSIter;
typedef RealRealPairSet::const_iterator        RRPSCIter;
typedef RealRealPairRealMap::iterator          RRPRMIter;
typedef RealRealPairRealMap::const_iterator    RRPRMCIter;
typedef IntRealVectorMap::iterator             IntRVMIter;
typedef IntRealVectorMap::const_iterator       IntRVMCIter;
typedef IntRealMatrixMap::iterator             IntRMMIter;
typedef IntRealMatrixMap::const_iterator       IntRMMCIter;
typedef IntActiveSetMap::iterator              IntASMIter;
typedef IntVariablesMap::iterator              IntVarsMIter;
typedef IntVariablesMap::const_iterator        IntVarsMCIter;
typedef IntResponseMap::iterator               IntRespMIter;
typedef IntResponseMap::const_iterator         IntRespMCIter;

} // namespace Dakota

#endif // DATA_TYPES_H


#include <highgui.h>
#include <Eigen/Eigen>

namespace ndt_feature_reg
{

template <typename PointT>
void ExpNDTFrameProc<PointT>::addFrameIncremental (NDTFrame<PointT> *f, bool skipMatching, bool ndtEstimateDI,
        bool match_full, bool match_no_association)
{

    if(!match_full)
    {
        detectKeypoints(f);
    }

    f->computeNDT(ndtEstimateDI, this->non_mean);

    if(!(match_no_association||match_full))
    {
        f->assignPts();
        calcDescriptors(f);
    }
    frames.push_back(f);

    ExpNDTFrameProc<PointT>::EigenTransform tr, tr2;
    tr.setIdentity();
    if(frames.size() == 1)
    {

        transformVector.clear();
        transformVector.push_back(tr);
        return;
    }

    Eigen::Affine3d transl_transform;
    Eigen::Affine3d rot_transform;
    std::vector<std::pair<int,int> > corr;

    int i = frames.size()-1;

    if(!(match_full || match_no_association))
    {
        pe.estimate(*frames[0],*frames[i]);
        transl_transform = (Eigen::Affine3d)Eigen::Translation3d(pe.trans);
        rot_transform = (Eigen::Affine3d)Eigen::Matrix3d(pe.rot);
        tr = transl_transform*rot_transform;
	std::cout<<pe.inliers.size()<<std::endl;
        if (!skipMatching)
        {
            corr.clear();
            corr = convertMatches(pe.inliers);
            lslgeneric::NDTMatcherFeatureD2D<PointT,PointT> matcher_feat(corr, trim_factor);
            matcher_feat.match(frames[0]->ndt_map, frames[i]->ndt_map, tr, true); //true = use initial guess
            //std::cout<<tr.rotation()<<"\n"<<tr.translation().transpose()<<std::endl;
        }
    }
    else
    {
        lslgeneric::NDTMatcherD2D<PointT,PointT> matcher;
        matcher.current_resolution = frames[0]->current_res/2;
        matcher.match(frames[0]->ndt_map, frames[i]->ndt_map, tr);
        //std::cout<<tr.rotation()<<"\n"<<tr.translation().transpose()<<std::endl;
    }

    //tr2 = tr*transformVector.back().inverse();

    transformVector.push_back(tr);

}

template <typename PointT>
void ExpNDTFrameProc<PointT>::trimNbFrames (size_t maxNbFrames)
{
    if (frames.size() > maxNbFrames)
    {
        //frames.erase(frames.begin(), frames.begin() + frames.size() - maxNbFrames);
        for(size_t i =1; i<frames.size(); i++)
        {
            //std::cout<<"delete f : "<<i<<std::endl;
            delete frames[i];
        }
        frames.erase(frames.begin()+1, frames.end());
        transformVector.erase(transformVector.begin()+1, transformVector.end()); // Just to keep a 1-1 coorespondance with frames.
    }
}

template <typename PointT>
void
ExpNDTFrameProc<PointT>::detectKeypoints(NDTFrame<PointT> *f) const
{
    if (img_scale == 1.)
    {
        detector->detect(f->img, f->kpts);
    }
    else
    {
        cv::Mat tmp;
        cv::resize(f->img, tmp, cv::Size(), img_scale, img_scale, cv::INTER_LINEAR);
        detector->detect(tmp, f->kpts);
        float factor = 1./img_scale;
        scaleKeyPointPosition(f->kpts, factor);
    }
}

template <typename PointT>
void
ExpNDTFrameProc<PointT>::calcDescriptors(NDTFrame<PointT> *f) const
{
    if (img_scale == 1.)
    {
	//create image with copied out pixels
	double dsize = f->kpts.size();
	int N_DIMS = ceil(sqrt(dsize));
	//std::cout<<dsize<<" "<<N_DIMS<<std::endl;
	int support_size_max = 0;
	for (unsigned int q=0; q<f->kpts.size(); ++q) {
	    support_size_max = (f->kpts[q].size > support_size_max) ? f->kpts[q].size : support_size_max;
	}
	std::cout<<"support_size_max = "<<support_size_max<<std::endl;

	int support_size;
	cv::Mat projected (support_size_max*N_DIMS,support_size_max*N_DIMS,f->img.type());
	cv::Mat projected_drawn (support_size_max*N_DIMS,support_size_max*N_DIMS,f->img.type());
	cv::Mat orig_drawn (f->img.size(),f->img.type());
	int i, j, p, k;
	std::vector<cv::KeyPoint> new_kpts;
	cv::KeyPoint kp;
	cv::Size sz;

	for (unsigned int q=0; q<f->kpts.size(); ++q) {
	    support_size = (f->kpts[q].size > support_size_max) ? support_size_max : f->kpts[q].size;

	    i = support_size_max * (q / N_DIMS);
	    j = support_size_max * (q % N_DIMS);
	    cv::Mat roiMat (projected,cv::Rect(i,j,support_size,support_size));
	    p = f->kpts[q].pt.x - support_size/2;
	    k = f->kpts[q].pt.y - support_size/2;
	    cv::Mat roi2Mat(f->img,cv::Rect(p,k,support_size,support_size));
	    roi2Mat.copyTo(roiMat);
	    kp = f->kpts[q];
	    kp.pt.x = i+support_size/2;
	    kp.pt.y = j+support_size/2;
	    kp.size = support_size;
	    new_kpts.push_back(kp);
	}

	cv::drawKeypoints(projected,new_kpts,projected_drawn,cv::Scalar::all(-1),4);
	cv::namedWindow("projected",0);
	cv::imshow("projected", projected_drawn);

	cv::drawKeypoints(f->img,f->kpts,orig_drawn,cv::Scalar::all(-1),4);
	cv::namedWindow("original",0);
	cv::imshow("original", orig_drawn);

	cv::waitKey(0);

	cv::Mat temptors;
	std::cout<<"valid kpts: "<<f->kpts.size()<<" "<<new_kpts.size()<<std::endl;
        extractor->compute(f->img, f->kpts, f->dtors);
        extractor->compute(projected, new_kpts, temptors);
	std::cout<<"after kpts: "<<f->kpts.size()<<" "<<new_kpts.size()<<std::endl;
    }
    else
    {
        cv::Mat tmp;
        cv::resize(f->img, tmp, cv::Size(), img_scale, img_scale, cv::INTER_LINEAR);
        float factor = img_scale;
        scaleKeyPointPosition(f->kpts, factor);
        extractor->compute(tmp, f->kpts, f->dtors);
        factor = 1./img_scale;
        scaleKeyPointPosition(f->kpts, factor);
    }
}

}


//==============================================================================
//         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_IEEE_FUNCTIONS_SIMD_COMMON_FAST_FREXP_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_FAST_FREXP_HPP_INCLUDED
#include <boost/simd/toolbox/ieee/functions/fast_frexp.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/fusion/tuple.hpp>
#include <boost/simd/include/functions/simd/shri.hpp>
#include <boost/simd/include/functions/simd/bitwise_andnot.hpp>
#include <boost/fusion/include/vector.hpp>
#include <boost/simd/include/constants/maxexponent.hpp>
#include <boost/simd/include/constants/nbmantissabits.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF( boost::simd::tag::fast_frexp_, tag::cpu_, (A0)(A1)(X)
                            , ( boost::is_same<typename dispatch::meta::as_integer<A0>::type, A1> )
                            , ((simd_< floating_<A0>, X>))
                              ((simd_< floating_<A0>, X>))
                              ((simd_< integer_<A1>, X>))
                            )
  {
    typedef int result_type;
    inline int operator()(A0 const& a0,A0 & r0,A1 & r1) const
    {
      typedef typename dispatch::meta::as_integer<A0, signed>::type      int_type;
      typedef typename meta::scalar_of<int_type>::type        sint_type;
      typedef typename meta::scalar_of<A0>::type                 s_type;
      const sint_type me = Maxexponent<s_type>()-1;
      const sint_type nmb= Nbmantissabits<s_type>();
      const int_type vn1 = boost::simd::splat<int_type>((2*me+3)<<nmb);
      const sint_type n2 = me<<nmb;
      r1 = b_and(vn1, a0);                                 //extract exponent
      A0 x = b_andnot(a0, vn1);                            //clear exponent in a0
      r1 = shri(r1,nmb) - splat<int_type>(me);             //compute exponent
      r0 = b_or(x,splat<int_type>(n2));                    //insert expon+1 in x
      return 0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION_IF( boost::simd::tag::fast_frexp_, tag::cpu_, (A0)(A1)(X)
                                 , ( boost::is_same<typename dispatch::meta::as_integer<A0>::type, A1>)
                                 , ((simd_< floating_<A0>, X>))
                                   ((simd_< integer_<A1>, X>))
                            )
  {
    typedef A0 result_type;
    inline A0 operator()(A0 const& a0,A1 & a2) const
    {
      A0 a1;
      boost::simd::fast_frexp(a0, a1, a2);
      return a1;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::fast_frexp_, tag::cpu_,
                             (A0)(X),
                             ((simd_<arithmetic_<A0>,X>))
                            )
  {
    typedef typename dispatch::meta::as_integer<A0, signed>::type  exponent;
    typedef boost::fusion::vector<A0,exponent>        result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(1)
    {
      result_type res;
      boost::simd::fast_frexp( a0
          , boost::fusion::at_c<0>(res)
          , boost::fusion::at_c<1>(res)
          );
      return res;
    }
  };
} } }
#endif


/*
 * Copyright META1 (c) 2020
 */

#include <random>

#include <boost/test/unit_test.hpp>

#include <graphene/chain/hardfork.hpp>
#include <graphene/chain/market_object.hpp>

#include "../common/meta1_fixture.hpp"

using namespace graphene::chain;
using namespace graphene::chain::test;

/**
 * Test of maximum supply of non-core assets
 */
BOOST_FIXTURE_TEST_SUITE(max_supply_tests, meta1_fixture)

   /**
    * Create a market-issued asset (MIA) with a maximum supply equal to GRAPHENE_MAX_SHARE_SUPPLY
    */
   BOOST_AUTO_TEST_CASE(create_mia_with_max_supply) {
      try {
         /**
          * Initialize
          */
         ACTORS((nathan)(dan)(ben)(vikram));

         /**
          * Advance past hardfork
          */
         generate_blocks(HARDFORK_CORE_1465_TIME);
         set_expiration(db, trx);

         /**
          * Create the bitasset
          *
          * 1 CORE = 10^5 satoshi CORE
          * 1 LARGE = 1 satoshi LARGE (because precision = 0)
          */
         asset_id_type large_id = create_bitasset("LARGE", nathan_id, 100, charge_market_fee, 0U,
                                                  asset_id_type{}, GRAPHENE_MAX_SHARE_SUPPLY).id;
         BOOST_CHECK_EQUAL(0, large_id(db).dynamic_data(db).current_supply.value);
         BOOST_CHECK_EQUAL(GRAPHENE_MAX_SHARE_SUPPLY, large_id(db).options.max_supply.value);

         generate_block();
         trx.clear();
         set_expiration(db, trx);

         /**
          * Define the feed publishers
          */
         {
            asset_update_feed_producers_operation op;
            op.asset_to_update = large_id;
            op.issuer = nathan_id;
            op.new_feed_producers = {dan_id, ben_id, vikram_id};
            trx.operations.push_back(op);
            sign(trx, nathan_private_key);
            PUSH_TX(db, trx);
         }

         generate_block();
         trx.clear();
         set_expiration(db, trx);
         large_id = get_asset("LARGE").id;
         asset_id_type core_id = get_asset(GRAPHENE_SYMBOL).id;

         /**
          * Publish a feed price
          * 1 satoshi CORE = 5000 satoshi LARGE --> 10^5 CORE = 5*10^8 LARGE
          */
         {
            price_feed current_feed;
            current_feed.settlement_price = large_id(db).amount(5000) / core_id(db).amount(1);
            current_feed.maintenance_collateral_ratio = 1750; // Need to set this explicitly, testnet has a different default
            publish_feed(large_id(db), vikram, current_feed);
         }

         /**
          * Borrow all the LARGE into existence
          */
         {
            const int64_t GRAPHENE_CORE_MAX_SHARE_SUPPLY= get_asset(GRAPHENE_SYMBOL).options.max_supply.value;
            transfer(committee_account, dan_id, asset(GRAPHENE_CORE_MAX_SHARE_SUPPLY/3));
            borrow(dan_id(db), large_id(db).amount(GRAPHENE_MAX_SHARE_SUPPLY),
                   core_id(db).amount(GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 2)); // CR = 2

            BOOST_CHECK_EQUAL(GRAPHENE_MAX_SHARE_SUPPLY, large_id(db).dynamic_data(db).current_supply.value);
            BOOST_REQUIRE_EQUAL(get_balance(dan, large_id(db)), GRAPHENE_MAX_SHARE_SUPPLY);
            BOOST_REQUIRE_EQUAL(get_balance(dan, core_id(db)),
                                GRAPHENE_CORE_MAX_SHARE_SUPPLY/3 - GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 2);
         }

         // The supply of the MIA should be at the maximum value
         BOOST_CHECK_EQUAL(large_id(db).dynamic_asset_data_id(db).current_supply.value, GRAPHENE_MAX_SHARE_SUPPLY);

         generate_block();
      }
      FC_LOG_AND_RETHROW()
   }

   /**
    * Test a market sale of the entire supply in a limit order
    */
   BOOST_AUTO_TEST_CASE(sell_max_supply_1) {
      INVOKE(create_mia_with_max_supply);

      /**
       * Re-initialize
       */
      trx.clear();
      set_expiration(db, trx);
      asset_id_type core_id = get_asset(GRAPHENE_SYMBOL).id;
      asset_id_type large_id = get_asset("LARGE").id;
      GET_ACTOR(dan);

      const int64_t GRAPHENE_CORE_MAX_SHARE_SUPPLY= get_asset(GRAPHENE_SYMBOL).options.max_supply.value;
      BOOST_REQUIRE_EQUAL(get_balance(dan, large_id(db)), GRAPHENE_MAX_SHARE_SUPPLY);
      BOOST_REQUIRE_EQUAL(get_balance(dan, core_id(db)),
                          GRAPHENE_CORE_MAX_SHARE_SUPPLY / 3 - GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 2);

      /**
       * Dan places an order to sell all the LARGE for 10 CORE
       */
      limit_order_object dan_sell_all
         = *create_sell_order(dan, large_id(db).amount(GRAPHENE_MAX_SHARE_SUPPLY), core_id(db).amount(1));
      limit_order_id_type dan_sell_all_id = dan_sell_all.id;
      // The order should remain on the books without being matched
      BOOST_REQUIRE_EQUAL(get_balance(dan, large_id(db)), 0); // Dan's balance should be zero
      BOOST_REQUIRE_EQUAL(get_balance(dan, core_id(db)),
                          GRAPHENE_CORE_MAX_SHARE_SUPPLY / 3 - (GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 2));

      /**
       * Yanna places an order to sell 1 CORE for 5 LARGE
       */
      ACTOR(yanna);
      const asset yanna_initial_core_balance = core_id(db).amount(3 * 10000);
      trx.clear();
      fund(yanna, yanna_initial_core_balance);

      create_sell_order(yanna, core_id(db).amount(1), large_id(db).amount(5));

      /**
       * Check the order match and fill
       * Yanna's order should be matched to Dan's.
       * Yanna's order should be filled.  Dan's order should be partially filled.
       * The issue should collect 1% of the fill.
       */
      const uint64_t expected_fill_large = GRAPHENE_MAX_SHARE_SUPPLY;
      const uint64_t expected_issuer_fee_large = GRAPHENE_MAX_SHARE_SUPPLY / 100; // Asset's market fee is 1%
      const uint64_t expected_receipt_large = expected_fill_large - expected_issuer_fee_large;

      BOOST_REQUIRE_EQUAL(get_balance(yanna, large_id(db)), expected_receipt_large);
      BOOST_REQUIRE_EQUAL(get_balance(yanna, core_id(db)), (3 * 10000) - 1);

      BOOST_REQUIRE_EQUAL(get_balance(dan, large_id(db)), 0); // Dan's balance should be zero
      BOOST_REQUIRE_EQUAL(get_balance(dan, core_id(db)),
                          GRAPHENE_CORE_MAX_SHARE_SUPPLY / 3 - (GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 2) + 1);

      // Check the asset issuer's accumulated fees
      BOOST_CHECK(large_id(db).dynamic_asset_data_id(db).accumulated_fees == expected_issuer_fee_large);

   }

   /**
    * Test a margin call of the entire supply that SHOULD trigger a global settlement
    * because of **the absence of offers** to purchase the margin call
    */
   BOOST_AUTO_TEST_CASE(margin_call_global_settlement_1) {
      INVOKE(create_mia_with_max_supply);

      /**
       * Re-initialize
       */
      trx.clear();
      set_expiration(db, trx);
      asset_id_type core_id = get_asset(GRAPHENE_SYMBOL).id;
      asset_id_type large_id = get_asset("LARGE").id;
      GET_ACTOR(dan);

      const int64_t GRAPHENE_CORE_MAX_SHARE_SUPPLY= get_asset(GRAPHENE_SYMBOL).options.max_supply.value;
      BOOST_REQUIRE_EQUAL(get_balance(dan, large_id(db)), GRAPHENE_MAX_SHARE_SUPPLY);
      BOOST_REQUIRE_EQUAL(get_balance(dan, core_id(db)),
                          GRAPHENE_CORE_MAX_SHARE_SUPPLY / 3 - GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 2);

      BOOST_CHECK(!large_id(db).bitasset_data(db).has_settlement()); // No global settlement yet

      /**
       * Publish a feed price that should trigger a margin call by increasing the MCR
       * 1 satoshi CORE = 5000 satoshi LARGE --> 10^5 CORE = 5*10^8 LARGE
       */
      {
         // Publish a new feed that requires a higher MCR than is available in the outstanding debt position.
         // It should be sufficient to trigger a margin call.
         // However, there are no counter-offers that can match and completely fill the offer.
         // Furthermore, because the 200% CR of the outstanding debt position is below the NEW MSSR of 300%,
         // a global settlement should be triggered.
         GET_ACTOR(vikram);
         price_feed current_feed;
         current_feed.settlement_price = large_id(db).amount(5000) / core_id(db).amount(1);
         current_feed.maintenance_collateral_ratio = 1750 * 2; // Initially 1750
         current_feed.maximum_short_squeeze_ratio = 3000;
         publish_feed(large_id(db), vikram, current_feed);

      }

      /**
       * Check the state
       */
      // A global settlement should have been triggered
      BOOST_CHECK(large_id(db).bitasset_data(db).has_settlement());
   }

   /**
    * Test a margin call of the entire supply that SHOULD trigger a global settlement
    * because of an existing offer, **created in the same block** as the updated price feed,
    * to buy the margin call **will not match because it is too high**
    */
   BOOST_AUTO_TEST_CASE(margin_call_global_settlement_2a) {
      INVOKE(create_mia_with_max_supply);

      /**
       * Re-initialize
       */
      trx.clear();
      set_expiration(db, trx);
      asset_id_type core_id = get_asset(GRAPHENE_SYMBOL).id;
      asset_id_type large_id = get_asset("LARGE").id;
      GET_ACTOR(dan);

      const int64_t GRAPHENE_CORE_MAX_SHARE_SUPPLY= get_asset(GRAPHENE_SYMBOL).options.max_supply.value;
      BOOST_REQUIRE_EQUAL(get_balance(dan, large_id(db)), GRAPHENE_MAX_SHARE_SUPPLY);
      BOOST_REQUIRE_EQUAL(get_balance(dan, core_id(db)),
                          GRAPHENE_CORE_MAX_SHARE_SUPPLY / 3 - GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 2);

      BOOST_CHECK(!large_id(db).bitasset_data(db).has_settlement()); // No global settlement yet

      /**
       * Create a sell order of CORE for the MIA that is should match the future margin call
       * and be large enough to completely fill the margin call.
       * Dan places an order to sell all of his borrowed LARGE at a price of
       * 1 satoshi LARGE = 5/5000 satoshi CORE --> 5000 satoshi LARGE = 5 satoshi CORE --> 5000 LARGE = 5*10^-5 CORE
       * = 5 * feed_price denominated in CORE/LARGE
       */
      trx.clear();
      limit_order_object dan_sell_order
         = *create_sell_order(dan, large_id(db).amount(GRAPHENE_MAX_SHARE_SUPPLY),
                              core_id(db).amount(GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 5));
      limit_order_id_type dan_sell_order_id = dan_sell_order.id;

      /**
       * Publish a feed price that should trigger a margin call by increasing the MCR
       * 1 satoshi CORE = 5000 satoshi LARGE --> 10^5 CORE = 5*10^8 LARGE
       */
      {
         // Publish a new feed that requires a higher MCR than is available in the outstanding debt position.
         // It should be sufficient to trigger a margin call.
         // However, there are no counter-offers that can match and completely fill the offer.
         // Furthermore, because the 200% CR of the outstanding debt position is below the NEW MSSR of 300%,
         // a global settlement should be triggered.
         GET_ACTOR(vikram);
         price_feed current_feed;
         current_feed.settlement_price = large_id(db).amount(5000) / core_id(db).amount(1);
         current_feed.maintenance_collateral_ratio = 1750 * 2; // Initially 1750
         current_feed.maximum_short_squeeze_ratio = 3000;
         publish_feed(large_id(db), vikram, current_feed);

      }

      /**
       * Check the state
       */
      // A global settlement should have been triggered
      BOOST_CHECK(large_id(db).bitasset_data(db).has_settlement());
   }

   /**
    * Test a margin call of the entire supply that SHOULD trigger a global settlement
    * because of an existing offer, **created in a previous block** as the updated price feed,
    * to buy the margin call **will not match because it is too high**
    */
   BOOST_AUTO_TEST_CASE(margin_call_global_settlement_2b) {
      INVOKE(create_mia_with_max_supply);

      /**
       * Re-initialize
       */
      trx.clear();
      set_expiration(db, trx);
      asset_id_type core_id = get_asset(GRAPHENE_SYMBOL).id;
      asset_id_type large_id = get_asset("LARGE").id;
      GET_ACTOR(dan);

      const int64_t GRAPHENE_CORE_MAX_SHARE_SUPPLY= get_asset(GRAPHENE_SYMBOL).options.max_supply.value;
      BOOST_REQUIRE_EQUAL(get_balance(dan, large_id(db)), GRAPHENE_MAX_SHARE_SUPPLY);
      BOOST_REQUIRE_EQUAL(get_balance(dan, core_id(db)),
                          GRAPHENE_CORE_MAX_SHARE_SUPPLY / 3 - GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 2);

      BOOST_CHECK(!large_id(db).bitasset_data(db).has_settlement()); // No global settlement yet

      /**
       * Create a sell order of CORE for the MIA that is should match the future margin call
       * and be large enough to completely fill the margin call.
       * Dan places an order to sell all of his borrowed LARGE at a price of
       * 1 satoshi LARGE = 5/5000 satoshi CORE --> 5000 satoshi LARGE = 5 satoshi CORE --> 5000 LARGE = 5*10^-5 CORE
       * = 5 * feed_price denominated in CORE/LARGE
       */
      trx.clear();
      limit_order_object dan_sell_order
         = *create_sell_order(dan, large_id(db).amount(GRAPHENE_MAX_SHARE_SUPPLY),
                              core_id(db).amount(GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 5));
      limit_order_id_type dan_sell_order_id = dan_sell_order.id;

      /**
       * Generate a block between the limit order and the updated price feed
       */
      generate_block();

      /**
       * Publish a feed price that should trigger a margin call by increasing the MCR
       * 1 satoshi CORE = 5000 satoshi LARGE --> 10^5 CORE = 5*10^8 LARGE
       */
      {
         // Publish a new feed that requires a higher MCR than is available in the outstanding debt position.
         // It should be sufficient to trigger a margin call.
         // However, there are no counter-offers that can match and completely fill the offer.
         // Furthermore, because the 200% CR of the outstanding debt position is below the NEW MSSR of 300%,
         // a global settlement should be triggered.
         GET_ACTOR(vikram);
         price_feed current_feed;
         current_feed.settlement_price = large_id(db).amount(5000) / core_id(db).amount(1);
         current_feed.maintenance_collateral_ratio = 1750 * 2; // Initially 1750
         current_feed.maximum_short_squeeze_ratio = 3000;
         publish_feed(large_id(db), vikram, current_feed);

      }

      /**
       * Check the state
       */
      // A global settlement should have been triggered
      BOOST_CHECK(large_id(db).bitasset_data(db).has_settlement());
   }

   /**
    * Test a margin call of the entire supply that should NOT trigger a global settlement
    * because there exists an offer, **created in the same block** as the updated price feed,
    * that can match and completely fill the margin call
    */
   BOOST_AUTO_TEST_CASE(margin_call_no_global_settlement_1a) {
      INVOKE(create_mia_with_max_supply);

      /**
       * Re-initialize
       */
      trx.clear();
      set_expiration(db, trx);
      asset_id_type core_id = get_asset(GRAPHENE_SYMBOL).id;
      asset_id_type large_id = get_asset("LARGE").id;
      GET_ACTOR(dan);

      const int64_t GRAPHENE_CORE_MAX_SHARE_SUPPLY= get_asset(GRAPHENE_SYMBOL).options.max_supply.value;
      BOOST_REQUIRE_EQUAL(get_balance(dan, large_id(db)), GRAPHENE_MAX_SHARE_SUPPLY);
      BOOST_REQUIRE_EQUAL(get_balance(dan, core_id(db)),
                          GRAPHENE_CORE_MAX_SHARE_SUPPLY / 3 - GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 2);

      BOOST_CHECK(!large_id(db).bitasset_data(db).has_settlement()); // No global settlement yet

      /**
       * Create a sell order of CORE for the MIA that is should match the future margin call
       * and be large enough to completely fill the margin call.
       * Dan places an order to sell all of his borrowed LARGE at a price of
       * 1 satoshi LARGE = 1/5000 satoshi CORE --> 5000 satoshi LARGE = 1 satoshi CORE --> 5000 LARGE = 1*10^-5 CORE
       * = 1 * feed_price denominated in CORE/LARGE
       */
      trx.clear();
      limit_order_object dan_sell_order
         = *create_sell_order(dan, large_id(db).amount(GRAPHENE_MAX_SHARE_SUPPLY),
                              core_id(db).amount(GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 1));
      limit_order_id_type dan_sell_order_id = dan_sell_order.id;

      /**
       * Publish a feed price that should trigger a margin call by increasing the MCR
       * The feed price will remain unchanged from the previous
       * 1 satoshi CORE = 5000 satoshi LARGE --> 10^5 CORE = 5*10^8 LARGE
       */
      {
         // Publish a new feed that requires a higher MCR than is available in the outstanding debt position
         // It should be sufficient to trigger a margin call.
         // However, the are existing counter-offers should match and completely fill the offer
         // to avoid a global settlement.
         GET_ACTOR(vikram);
         price_feed current_feed;
         current_feed.settlement_price = large_id(db).amount(5000) / core_id(db).amount(1);
         current_feed.maintenance_collateral_ratio = 1750 * 2; // Initially 1750
         current_feed.maximum_short_squeeze_ratio = 3000;
         publish_feed(large_id(db), vikram, current_feed);
      }

      /**
       * Dan's margin call should be matched against his own limit order.
       */
      // A global settlement should have been triggered
      BOOST_CHECK(!large_id(db).bitasset_data(db).has_settlement());

      // Confirm that the limit order no longer exists
      BOOST_CHECK(!db.find(dan_sell_order_id));

      // Dan's balances should be restored to their initial values
      BOOST_REQUIRE_EQUAL(get_balance(dan, large_id(db)), 0);
      BOOST_REQUIRE_EQUAL(get_balance(dan, core_id(db)), GRAPHENE_CORE_MAX_SHARE_SUPPLY/3);

      // The supply of the MIA should be zero
      BOOST_CHECK_EQUAL(large_id(db).dynamic_asset_data_id(db).current_supply.value, 0);

   }

   /**
    * Test a margin call of the entire supply that should NOT trigger a global settlement
    * because there exists an offer, **created in a previous block** as the updated price feed,
    * that can match and completely fill the margin call
    */
   BOOST_AUTO_TEST_CASE(margin_call_no_global_settlement_1b) {
      INVOKE(create_mia_with_max_supply);

      /**
       * Re-initialize
       */
      trx.clear();
      set_expiration(db, trx);
      asset_id_type core_id = get_asset(GRAPHENE_SYMBOL).id;
      asset_id_type large_id = get_asset("LARGE").id;
      GET_ACTOR(dan);

      const int64_t GRAPHENE_CORE_MAX_SHARE_SUPPLY= get_asset(GRAPHENE_SYMBOL).options.max_supply.value;
      BOOST_REQUIRE_EQUAL(get_balance(dan, large_id(db)), GRAPHENE_MAX_SHARE_SUPPLY);
      BOOST_REQUIRE_EQUAL(get_balance(dan, core_id(db)),
                          GRAPHENE_CORE_MAX_SHARE_SUPPLY / 3 - GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 2);

      BOOST_CHECK(!large_id(db).bitasset_data(db).has_settlement()); // No global settlement yet

      /**
       * Create a sell order of CORE for the MIA that is should match the future margin call
       * and be large enough to completely fill the margin call.
       * Dan places an order to sell all of his borrowed LARGE at a price of
       * 1 satoshi LARGE = 1/5000 satoshi CORE --> 5000 satoshi LARGE = 1 satoshi CORE --> 5000 LARGE = 1*10^-5 CORE
       * = 1 * feed_price denominated in CORE/LARGE
       */
      trx.clear();
      limit_order_object dan_sell_order
         = *create_sell_order(dan, large_id(db).amount(GRAPHENE_MAX_SHARE_SUPPLY),
                              core_id(db).amount(GRAPHENE_MAX_SHARE_SUPPLY / 5000 * 1));
      limit_order_id_type dan_sell_order_id = dan_sell_order.id;

      /**
       * Generate a block between the limit order and the updated price feed
       */
       generate_block();

      /**
       * Publish a feed price that should trigger a margin call by increasing the MCR
       * The feed price will remain unchanged from the previous
       * 1 satoshi CORE = 5000 satoshi LARGE --> 10^5 CORE = 5*10^8 LARGE
       */
      {
         // Publish a new feed that requires a higher MCR than is available in the outstanding debt position
         // It should be sufficient to trigger a margin call.
         // However, the are existing counter-offers should match and completely fill the offer
         // to avoid a global settlement.
         GET_ACTOR(vikram);
         price_feed current_feed;
         current_feed.settlement_price = large_id(db).amount(5000) / core_id(db).amount(1);
         current_feed.maintenance_collateral_ratio = 1750 * 2; // Initially 1750
         current_feed.maximum_short_squeeze_ratio = 3000;
         publish_feed(large_id(db), vikram, current_feed);
      }

      /**
       * Dan's margin call should be matched against his own limit order.
       */
      // A global settlement should have been triggered
      BOOST_CHECK(!large_id(db).bitasset_data(db).has_settlement());

      // Confirm that the limit order no longer exists
      BOOST_CHECK(!db.find(dan_sell_order_id));

      // Dan's balances should be restored to their initial values
      BOOST_REQUIRE_EQUAL(get_balance(dan, large_id(db)), 0);
      BOOST_REQUIRE_EQUAL(get_balance(dan, core_id(db)), GRAPHENE_CORE_MAX_SHARE_SUPPLY/3);

      // The supply of the MIA should be zero
      BOOST_CHECK_EQUAL(large_id(db).dynamic_asset_data_id(db).current_supply.value, 0);

   }

BOOST_AUTO_TEST_SUITE_END()