#include "Controller_Generic.h"
// #include <Eigen/Dense>
// #include <iostream>

using namespace Eigen;

namespace DQ_robotics
{

//----------------------------------------------------------------------------------------------------------
//##########################################################################################################
//#################################================================#########################################
//#################################                                #########################################
//#################################  CLASS : Controller_Generic    #########################################
//#################################                                #########################################
//#################################================================#########################################
//##########################################################################################################
//----------------------------------------------------------------------------------------------------------
/// * CALL init_default_parameters with robot input (DQ_kinematics)
Controller_Generic::Controller_Generic(DQ_kinematics robot) : DQ_controller()
{
    init_default_parameters(robot);
    print_parameters();
}

/**********************************************************************************
####################################################################################
####  CLASS:   Controller_Generic
####  FUNCTION:   init_default_parameters(DQ_kinematics robot)
####_______________________________________________________________________________
    * SET DEFAULT VALUES FOR THE PARAMETERS OF THE CLASS
####################################################################################
***********************************************************************************/
void Controller_Generic::init_default_parameters(DQ_kinematics robot)
{
    //** DEBUG MODE:
    var_FLAG_DEBUG__MODE_JOINTLIMIT_VERIF = true;

    //** SIZE OF THE TASK
    var_TASK_SIZE = 8;

    ///** Constant variables
    var_const_DQ_1         = DQ(1);
    var_const_Identity_4x4 = Matrix<double,4,4>::Identity();
    var_const_Identity_8x8 = Matrix<double,8,8>::Identity();

    //** Init Flags   ****[ DEFAULT VALUES ]
    var_FLAG_CTRL__at_least_one_error     = false;
    var_FLAG_CTRL__at_least_one_reference = false;
    var_FLAG_TRACK_enable_tracking_term   = false;
    var_FLAG_ROBOT__joint_limits          = false;

    //** Init Robotic Arm Param
    var_ROBOT_DOFs     = (robot.links() - robot.n_dummy());
    var_ROBOT_KINE     = robot;
    var_ROBOT_JOINTS_LIM__UPPER    = VectorXd::Zero(var_ROBOT_DOFs); // upper_joint_limits;
    var_ROBOT_JOINTS_LIM__LOWER    = VectorXd::Zero(var_ROBOT_DOFs); // lower_joint_limits;
    var_ROBOT_DUMMY_JOINTS         = var_ROBOT_KINE.dummy();

    //** Control Param ****[ DEFAULT VALUES ]
    var_ctrl_Kp     = MatrixXd::Identity(8,8);
    var_ctrl_Kp     = 0.1*var_ctrl_Kp;
    var_ctrl_Ki     = MatrixXd::Zero(8,8);
    var_ctrl_Kd     = MatrixXd::Zero(8,8);
    var_ctrl_Ki_memorySize = 0.99;
    var_ctrl_sri_lambda       = 0.01;
    var_ctrl_srivar_lambda_max = 0.0; //0.05;
    var_ctrl_srivar_ballsize  = 0.001;

    //** Init evolution Task variables
    var_task_thetas         = MatrixXd(var_ROBOT_DOFs,1);
    var_task_thetas_Delta   = MatrixXd::Zero(var_ROBOT_DOFs,1);
    var_task_ERROR          = MatrixXd(8,1);
    var_task_ki_error       = MatrixXd::Zero(8,1);
    var_task_kd_last_error  = MatrixXd::Zero(8,1);

    // // TASK JACOB
    varJACOB  = dqjacob(var_ROBOT_DOFs, 8);
    varJACOB4 = dqjacob(var_ROBOT_DOFs, 4);

    //** Tracking Terms
    var_tracking_updateTerm      = MatrixXd::Zero(8,1);
    var_tracking_lastReference   = DQ(0,0,0,0,0,0,0,0);
    var_tracking4_updateTerm      = MatrixXd::Zero(4,1);
    var_tracking4_lastReference   = DQ(0,0,0,0);
}

//----------------------------------------------------------------------------------------------------------
//##########################################################################################################
//#################################================================#########################################
//#################################                                #########################################
//#################################  CLASS : Controller_Generic    #########################################
//#################################         CONFIGURATION          #########################################
//#################################                                #########################################
//#################################================================#########################################
//##########################################################################################################
//----------------------------------------------------------------------------------------------------------

/**********************************************************************************
####################################################################################
####  CLASS:   Controller_Generic
####  FUNCTION:   set_joint_limits (VectorXd:  upper_limits and lower_limits )
####_______________________________________________________________________________
    * SET the arm joint limits and enable joint limits exclusion
***********************************************************************************/
void Controller_Generic::set_joint_limits(VectorXd upper_joint_limits, VectorXd lower_joint_limits)
{
    var_FLAG_ROBOT__joint_limits  = true;
    var_ROBOT_JOINTS_LIM__UPPER   = upper_joint_limits;
    var_ROBOT_JOINTS_LIM__LOWER   = lower_joint_limits;
}

/**********************************************************************************
####################################################################################
####  CLASS:   Controller_Generic
####  FUNCTION:   set_control_gains (different options)
####_______________________________________________________________________________
    * Adjust control gains of the PID (just P matrix)
***********************************************************************************/
void Controller_Generic::set_control_gains(const MatrixXd& kp){
    var_ctrl_Kp             = kp;
}
/// * Adjust control gains of the PID (just Kp and Ki matrices)
void Controller_Generic::set_control_gains(const MatrixXd& kp, const MatrixXd& ki){
    var_ctrl_Kp             = kp;
    var_ctrl_Ki             = ki;
}
/// * Adjust control gains of the PID (Kp and Ki and Kd matrices)
void Controller_Generic::set_control_gains(const MatrixXd& kp, const MatrixXd& ki, const MatrixXd& kd){
    set_control_gains(kp, ki);
    var_ctrl_Kd             = kd;
}
/// * Adjust control gains of the PID (Kp and Ki and Kd matrices | and  integral memory it)
void Controller_Generic::set_control_gains(const MatrixXd& kp, const MatrixXd& ki, const double ki_memory, const MatrixXd& kd){
    set_control_gains(kp, ki, kd);
    var_ctrl_Ki_memorySize  = ki_memory;
}
/// * Adjust control gains of the PID (just P gain-scalar)
void Controller_Generic::set_control_gains(double kp){
    MatrixXd IDENTIDADE;
    IDENTIDADE = get_IDENT();
    var_ctrl_Kp             = kp*IDENTIDADE;
}
/// * Adjust control gains of the PID (just Kp and Ki gain-scalars)
void Controller_Generic::set_control_gains(double kp, double ki){
    MatrixXd IDENTIDADE;
    IDENTIDADE = get_IDENT();
    var_ctrl_Kp             = kp*IDENTIDADE;
    var_ctrl_Ki             = ki*IDENTIDADE;
}
/// * Adjust control gains of the PID (Kp and Ki and Kd gain-scalars)
void Controller_Generic::set_control_gains(double kp, double ki, double kd){
    MatrixXd IDENTIDADE;
    IDENTIDADE = get_IDENT();
    var_ctrl_Kp             = kp*IDENTIDADE;
    var_ctrl_Ki             = ki*IDENTIDADE;
    var_ctrl_Kd             = kd*IDENTIDADE;
}
/// * Adjust control gains of the PID (Kp and Ki and Kd gain-scalars | and  integral memory it)
void Controller_Generic::set_control_gains(double kp, double ki, const double ki_memory, double kd){
    set_control_gains(kp, ki, kd);
    var_ctrl_Ki_memorySize  = ki_memory;
}

/**********************************************************************************
####################################################################################
####  CLASS:   Controller_Generic
####  FUNCTION:   set_jacob_srivar_paramconst (double:  sri_lambda, srivar_lambda_max, sri_var_lambda_region)
####_______________________________________________________________________________
    * Adjust damping parameters for the SRI variable inverse
####################################################################################
***********************************************************************************/
void Controller_Generic::set_jacob_srivar_paramconst(const double& sri_lambda, const double& sriVar_lambda_max, const double& sriVar_ball_size)
{
    var_ctrl_sri_lambda          = sri_lambda;
    var_ctrl_srivar_lambda_max   = sriVar_lambda_max;
    var_ctrl_srivar_ballsize     = sriVar_ball_size;
}
/// * Adjust damping parameter for the SRI  inverse
void Controller_Generic::set_jacob_srivar_paramconst(const double& sri_lambda)
{
    var_ctrl_sri_lambda          = sri_lambda;
}
//***************************************************************************************************************

void Controller_Generic::print_parameters()
{

}

//----------------------------------------------------------------------------------------------------------
//##########################################################################################################
//#################################================================#########################################
//#################################                                #########################################
//#################################  CLASS : Controller_Generic    #########################################
//#################################         CONTROLLERS            #########################################
//#################################                                #########################################
//#################################================================#########################################
//##########################################################################################################
//----------------------------------------------------------------------------------------------------------
VectorXd Controller_Generic::getNewJointPositions( const DQ reference, const VectorXd thetas)
{
    var_task_thetas_Delta = getNewJointVelocities(reference, thetas, 1.0);
    // Send updated thetas to simulation
    return (var_task_thetas + var_task_thetas_Delta);
}

VectorXd Controller_Generic::getNewJointPositions( const DQ reference, const VectorXd thetas, double POS_GAIN)
{
    var_task_thetas_Delta = getNewJointVelocities(reference, thetas, POS_GAIN);
    // Send updated thetas to simulation
    return (var_task_thetas + var_task_thetas_Delta);
}
VectorXd Controller_Generic::getNewJointVelocities( const DQ reference, const VectorXd thetas)
{
    return getNewJointVelocities( reference, thetas, 1.0);
}

VectorXd Controller_Generic::getNewJointVelocities( const DQ reference, const VectorXd thetas, double POS_GAIN)
{
    var_task_thetas = thetas; //This is necessary for the getNewJointPositions to work

    //** Variables for Joint Limit Verification
    varPSEUDO_ROBOT = pseudoRobot();
    varPSEUDO_ROBOT.setData(var_ROBOT_KINE, var_ROBOT_DOFs, var_task_thetas);

    //** Local Control Variables
    DQ pose_Xd;
    DQ pose_Xm;
    DQ end_effector_pose_ = DQ(0,0,0,0,0,0,0,0);
    DQ pose_reference = reference;

    MatrixXd mtemp;
    Matrix<double,8,1> mtemp8;

    mtemp8 = pose_reference.vec8();
    pose_reference = pose_reference*(pose_reference.norm().inv());
    pose_Xd = DQ( mtemp8(0,0), mtemp8(1,0), mtemp8(2,0), mtemp8(3,0), 0, 0, 0,0);
    mtemp8 = pose_reference.translation().vec8();
    pose_Xd =  pose_Xd + POS_GAIN*0.5*E_*DQ( 0, mtemp8(1,0), mtemp8(2,0), mtemp8(3,0), 0, 0, 0,0 )*pose_Xd;

    // NULL SPACE VARIABLES
    //*************************************************
    dqjacob jacobtemp;     //  7 x 1
    MatrixXd NS_Z2;     //  7 x 1
    MatrixXd NS_OPT_FCT; // n x 1
    MatrixXd NS_JACOB;  //  n x 7
    VectorXd        NS_THETA_MEDIO;
    VectorXd        NS_MARGIN;

    jacobtemp  = dqjacob(var_ROBOT_DOFs, 8);

    NS_Z2 = MatrixXd::Zero(7,1);
    NS_OPT_FCT = MatrixXd::Zero(1,1);
    NS_JACOB   = MatrixXd::Zero(1,7);
    NS_THETA_MEDIO    = VectorXd::Zero(var_ROBOT_DOFs);
    NS_MARGIN    = VectorXd::Zero(var_ROBOT_DOFs);

    for(int i=0,j=0; i < var_ROBOT_DOFs; i++)   {
        NS_THETA_MEDIO(i) = 0.5*( var_ROBOT_JOINTS_LIM__UPPER(i) + var_ROBOT_JOINTS_LIM__LOWER(i) );
        NS_MARGIN(i) = 0.03*( var_ROBOT_JOINTS_LIM__UPPER(i) - var_ROBOT_JOINTS_LIM__LOWER(i) );
    }

    //*************************************************

    bool should_break_loop = false;
    while(not should_break_loop){

        //******  Calculate JACOB (N = Hminus8(x_d) * C8 * J  )  ******
        varJACOB.jacob = varPSEUDO_ROBOT.KINE.analyticalJacobian(varPSEUDO_ROBOT.thetas);
        varJACOB.jacob =  Hminus8(pose_Xd)*C8()*varJACOB.jacob;

        //****** Current POSE (given pseudo robot) ******
        end_effector_pose_ = varPSEUDO_ROBOT.KINE.fkm(varPSEUDO_ROBOT.thetas);
        mtemp8 = end_effector_pose_.vec8();
        pose_Xm = DQ( mtemp8(0,0), mtemp8(1,0), mtemp8(2,0), mtemp8(3,0), 0, 0, 0,0);
        end_effector_pose_ = end_effector_pose_*(end_effector_pose_.norm().inv());
        mtemp8 = end_effector_pose_.translation().vec8();
        pose_Xm =  pose_Xm + POS_GAIN*0.5*E_*DQ( 0, mtemp8(1,0), mtemp8(2,0), mtemp8(3,0), 0, 0, 0,0)*pose_Xm;

        //****** UPDATE ERROR  ********
        var_task_kd_last_error   = var_task_ERROR;
        var_task_ERROR           = vec8( var_const_DQ_1 - (pose_Xm.conj())*pose_Xd );
        var_task_ki_error = var_task_ERROR + var_ctrl_Ki_memorySize*var_task_ki_error;

        //******  INVERSE MATRIX [init] *******
        int current_step_relevant_dof = ( varPSEUDO_ROBOT.KINE.links() - varPSEUDO_ROBOT.KINE.n_dummy() );
        varJACOB.get_SVD(current_step_relevant_dof);
        // varJACOB.get_INV(var_ctrl_sri_lambda, var_ctrl_srivar_lambda_max, var_ctrl_srivar_ballsize);
        varJACOB.get_INV_left( var_ctrl_sri_lambda );

        //******  TRACKING ******
        var_tracking_updateTerm  = MatrixXd::Zero(8,1);
        if (var_FLAG_TRACK_enable_tracking_term)  {
            if( var_FLAG_CTRL__at_least_one_reference )
              var_tracking_updateTerm = Hminus8( pose_Xd - var_tracking_lastReference )*C8()*vec8( pose_Xm );
            else
              var_FLAG_CTRL__at_least_one_reference = true;
            var_tracking_lastReference = pose_Xd;
        }

        //******  OUTPUT CONTROL   ******
        if( var_FLAG_CTRL__at_least_one_error )
            varPSEUDO_ROBOT.thetas_delta = varJACOB.jacobInv*( var_ctrl_Kp*var_task_ERROR + var_ctrl_Ki*var_task_ki_error + var_ctrl_Kd*(var_task_ERROR - var_task_kd_last_error)  - var_tracking_updateTerm );
        else  {
          var_FLAG_CTRL__at_least_one_error = true;
          varPSEUDO_ROBOT.thetas_delta = varJACOB.jacobInv*( var_ctrl_Kp*var_task_ERROR + var_ctrl_Ki*var_task_ki_error  - var_tracking_updateTerm );
        }

        //**************************************************************************************************************[                  ]
        //**************************************************************************************************************[ NULL SPACE       ]
        //**************************************************************************************************************[  EXPERIMENTATION ]
        int NS_OPTIONS = 0;
        //******  Null space optimization for joint limits   ******

        if (NS_OPTIONS > 0)
            varJACOB.get_NullSpaceProjector();

        NS_OPT_FCT(0,0) = 0;
        double nsGAIN= 0.5;

        //*********************************===>   OPTION 1
        if (NS_OPTIONS==1)
        {
            double xgain[7];
            xgain[0]=8;            xgain[1]=6;
            xgain[2]=4;            xgain[3]=4;
            xgain[4]=2;            xgain[5]=1;
            xgain[6]=1;
            for(int i=0,j=0; i < var_ROBOT_DOFs; i++)   {
                NS_OPT_FCT(0,0) = NS_OPT_FCT(0,0) + 0.5*xgain[i]*( varPSEUDO_ROBOT.thetas(i)-NS_THETA_MEDIO(i) )*( varPSEUDO_ROBOT.thetas(i)-NS_THETA_MEDIO(i) );
                NS_JACOB(0,i) =  xgain[i]*(varPSEUDO_ROBOT.thetas(i)-NS_THETA_MEDIO(i) );
            }

            jacobtemp.jacob = NS_JACOB*( varJACOB.jacobNSproject );
            // NS_Z2 =  dqjacob::get_INV( NS_JACOB*( varJACOB.jacobNSproject ), var_ctrl_sri_lambda )*(  var_ctrl_Kp*NS_OPT_FCT - 1*NS_JACOB*varPSEUDO_ROBOT.thetas_delta );
            NS_Z2 = dqjacob::get_INV( jacobtemp.jacob, var_ctrl_sri_lambda )*(  0.025*NS_OPT_FCT - 0.5*NS_JACOB*varPSEUDO_ROBOT.thetas_delta );

            // // NEW OUTPUT WITH NULL SPACE
            // varPSEUDO_ROBOT.thetas_delta  =  varPSEUDO_ROBOT.thetas_delta + 0.0008*( varJACOB.jacobNSproject )*NS_Z2;
            // varPSEUDO_ROBOT.thetas_delta  =  varPSEUDO_ROBOT.thetas_delta + 0.0001*( varJACOB.jacobNSproject )*NS_Z2;
            varPSEUDO_ROBOT.thetas_delta  =  varPSEUDO_ROBOT.thetas_delta + 0.000025*( varJACOB.jacobNSproject )*NS_Z2;
        }

        //*********************************===>   OPTION 2
        if ( (NS_OPTIONS==2) || (NS_OPTIONS==3) )
        {
            double jointslimdiv = 0;

            for(int i=0,j=0; i < var_ROBOT_DOFs; i++)   {
                NS_JACOB(0,i)  = 0;
                jointslimdiv  = varPSEUDO_ROBOT.thetas(i)-var_ROBOT_JOINTS_LIM__LOWER(i);
                if ( jointslimdiv  < NS_MARGIN(i) ) {
                    NS_OPT_FCT(0,0) = NS_OPT_FCT(0,0) + nsGAIN*0.5*pow(  (1/NS_MARGIN(i))*( NS_MARGIN(i)-jointslimdiv)  ,2);
                    NS_JACOB(0,i) =  -nsGAIN*(   (1/NS_MARGIN(i))*std::abs(  NS_MARGIN(i) - jointslimdiv )    );
                    std::cout << "bottom: " << i << "=> (" << varPSEUDO_ROBOT.thetas(i) << " x " << var_ROBOT_JOINTS_LIM__LOWER(i)  << ")= "<< jointslimdiv <<" ==>  " <<  NS_JACOB(0,i) << std::endl;
                }
                else {
                    jointslimdiv  = var_ROBOT_JOINTS_LIM__UPPER(i)-varPSEUDO_ROBOT.thetas(i);
                    if ( jointslimdiv  < NS_MARGIN(i) ) {
                        NS_OPT_FCT(0,0) = NS_OPT_FCT(0,0) + nsGAIN*0.5*pow(   (1/NS_MARGIN(i))*( NS_MARGIN(i)-jointslimdiv )    ,2);
                        NS_JACOB(0,i) =  +nsGAIN*(  (1/NS_MARGIN(i))*std::abs( NS_MARGIN(i)-jointslimdiv )   );
                        std::cout << "top: " << i << "=> (" << varPSEUDO_ROBOT.thetas(i) << " x " << var_ROBOT_JOINTS_LIM__UPPER(i)  << ")= "<< jointslimdiv <<" ==>  " <<  NS_JACOB(0,i) << std::endl;
                    }
                    else
                    {
                        NS_OPT_FCT(0,0) = NS_OPT_FCT(0,0);
                        NS_JACOB(0,i) =  0.00;
                    }
                }
            }

            if (NS_OPTIONS==2)
            {
                jacobtemp.jacob = NS_JACOB*( varJACOB.jacobNSproject );
                NS_Z2 = dqjacob::get_INV( jacobtemp.jacob, var_ctrl_sri_lambda )*(  0.5*NS_OPT_FCT - 0.1*NS_JACOB*varPSEUDO_ROBOT.thetas_delta );

                // // NEW OUTPUT WITH NULL SPACE
                varPSEUDO_ROBOT.thetas_delta  =  varPSEUDO_ROBOT.thetas_delta + 0.5*( varJACOB.jacobNSproject )*NS_Z2;
                // varPSEUDO_ROBOT.thetas_delta  =  varPSEUDO_ROBOT.thetas_delta ; // + 1*( varJACOB.jacobNSproject )*NS_Z2;
                // varPSEUDO_ROBOT.thetas_delta  =  varPSEUDO_ROBOT.thetas_delta + 0.5*( varJACOB.jacobNSproject )*dqjacob::get_INV( NS_JACOB, 0 )*NS_OPT_FCT ;
            }
            else
            {
                // std::cout << "FCT: " << std::endl << NS_JACOB.transpose() << std::endl ;
                // varPSEUDO_ROBOT.thetas_delta  =  varPSEUDO_ROBOT.thetas_delta - (0.2)*NS_JACOB.transpose();
                varPSEUDO_ROBOT.thetas_delta  =  varPSEUDO_ROBOT.thetas_delta - (2)*NS_JACOB.transpose();
            }
            // extra info
            // jacobtemp.get_SVD(jacobtemp.jacob.rows());
        }
        if ( (NS_OPTIONS<0) || (NS_OPTIONS>3) )
            varPSEUDO_ROBOT.thetas_delta  =  varPSEUDO_ROBOT.thetas_delta;

        //
        //**************************************************************************************************************[               ]
        //**************************************************************************************************************[    SECTION    ]
        //**************************************************************************************************************[       END     ]

        //Update delta_thetas for FUTURE THETA calculation
        for(int i=0,j=0; i < var_ROBOT_DOFs; i++)   {
            if(varPSEUDO_ROBOT.dummy_joints_marker(i) == 0) {
                var_task_thetas_Delta(i) = varPSEUDO_ROBOT.thetas_delta(j);
                ++j;
            }
        }

        //******  OUTPUT THETAS (POSSIBLE)   ******
        varPSEUDO_ROBOT.thetas_output = var_task_thetas + var_task_thetas_Delta;

        // //Verify if loop should end
        should_break_loop = true;
        if (NS_OPTIONS==0)
        {
            if (var_FLAG_ROBOT__joint_limits)
                should_break_loop = joint_limit_verification_step();
            else
                should_break_loop = true;
        }

    }//While not should_break_loop

    return var_task_thetas_Delta;

}

 bool Controller_Generic::joint_limit_verification_step()
 {
    bool returnBreakLoop = true;
    int j=0;
    //For all joints
    for(int i = 0; i < var_ROBOT_DOFs; i++){

        //If joint is not yet marked for not being considered in the minimization
        if(varPSEUDO_ROBOT.dummy_joints_marker(i) == 0){

            //If the controller is trying to put a joint further than any of its limits
            if(    varPSEUDO_ROBOT.thetas_output(i) > var_ROBOT_JOINTS_LIM__UPPER(i)
                || varPSEUDO_ROBOT.thetas_output(i) < var_ROBOT_JOINTS_LIM__LOWER(i) )
            {
                //If the joint speed (delta) acts as to push the joint away from its limits
                //  we consider the velocity given normally
                if (   (varPSEUDO_ROBOT.thetas_output(i) > var_ROBOT_JOINTS_LIM__UPPER(i)  &&  var_task_thetas_Delta(i) < 0)
                    || (varPSEUDO_ROBOT.thetas_output(i) < var_ROBOT_JOINTS_LIM__LOWER(i) &&  var_task_thetas_Delta(i) > 0) )
                {
                    var_task_thetas_Delta(i) = varPSEUDO_ROBOT.thetas_delta(j);
                    varPSEUDO_ROBOT.thetas(j) = var_task_thetas(i);
                    ++j;
                    continue;
                }

                //If the joint was already saturated sometime ago
                double ep = 1.e-05;
                if (    var_task_thetas(i) > var_ROBOT_JOINTS_LIM__UPPER(i) - ep
                     || var_task_thetas(i) < var_ROBOT_JOINTS_LIM__LOWER(i) + ep){

                    varPSEUDO_ROBOT.dummy_joints_marker(i) = 1; //Mark it to be ignored in the minization
                    //std::cout << std::endl << "Joint " << i << " will be ignored in the next controller step.";
                    varPSEUDO_ROBOT.dh_matrix(4,i) = 1;          //Set matrix as dummy.
                    varPSEUDO_ROBOT.dh_matrix(0,i) = var_task_thetas(i); //Set matrix theta as a fixed value.
                    returnBreakLoop = false;
                    if (var_FLAG_DEBUG__MODE_JOINTLIMIT_VERIF)
                        std::cout << "[WARN]:[Eliminate Joint " << i <<"]: It has saturated its limits sometime ago. Value:" << var_task_thetas(i) << std::endl;
                }
                //If the joint was not yet saturated and the controller wants to saturate it
                else{
                    // Saturate the joint in this step.
                    if   ( varPSEUDO_ROBOT.thetas_output(i) > var_ROBOT_JOINTS_LIM__UPPER(i) ){
                        var_task_thetas_Delta(i) = var_ROBOT_JOINTS_LIM__UPPER(i) - var_task_thetas(i);
                        if (var_FLAG_DEBUG__MODE_JOINTLIMIT_VERIF)
                            std::cout << "[WARN]: Joint (" << i <<") is going to saturate its upper limits. Value:" <<  varPSEUDO_ROBOT.thetas_output(i) << std::endl;
                    }
                    else if ( varPSEUDO_ROBOT.thetas_output(i) < var_ROBOT_JOINTS_LIM__LOWER(i) ){
                        var_task_thetas_Delta(i) = var_ROBOT_JOINTS_LIM__LOWER(i) - var_task_thetas(i);
                        if (var_FLAG_DEBUG__MODE_JOINTLIMIT_VERIF)
                            std::cout << "[WARN]: Joint (" << i <<") is going to saturate its lower limits. Value:" <<  varPSEUDO_ROBOT.thetas_output(i) << std::endl;
                    }
                    else{
                        std::cout << std::endl << "Something is really wrong";
                    }
                    //The joint should still be considered in the minimizations.
                    varPSEUDO_ROBOT.thetas(j) = var_task_thetas(i);
                    ++j;
                }

            }
            //If the controller is not trying to put this joint further than any of its limits, we consider the velocity given normally
            else{
                var_task_thetas_Delta(i) = varPSEUDO_ROBOT.thetas_delta(j);
                varPSEUDO_ROBOT.thetas(j) = var_task_thetas(i);
                ++j;
            }
        }
        //If joint was marked to be ignored, it shall be ignored.
        else{
            var_task_thetas_Delta(i) = 0;
            if (var_FLAG_DEBUG__MODE_JOINTLIMIT_VERIF)
                std::cout << "[WARN]: Ignore move in Joint (" << i <<") due to joint limits" << std::endl;
        }

    }
    if( j == 0 ){
        std::cout << std::endl << "Robot will be unable to get out of this configuration using this controller.";
        var_task_thetas_Delta = VectorXd::Zero(var_ROBOT_DOFs);
        returnBreakLoop = true;
        return returnBreakLoop;
    }
    if(not returnBreakLoop){
        varPSEUDO_ROBOT.KINE =  DQ_kinematics(varPSEUDO_ROBOT.dh_matrix);  //Change DH
        varPSEUDO_ROBOT.KINE.set_base(var_ROBOT_KINE.base());
        varPSEUDO_ROBOT.KINE.set_effector(var_ROBOT_KINE.effector());
        varPSEUDO_ROBOT.thetas.conservativeResize(j);           //Resize pseudothetas
        if (var_FLAG_DEBUG__MODE_JOINTLIMIT_VERIF)
            std::cout << "=========================================================" << std::endl;
    }
    return returnBreakLoop;

    std::cout << std::endl;

}

void Controller_Generic::enableTrackingTerm(const bool bool_falseortrue)
{
    var_FLAG_TRACK_enable_tracking_term = bool_falseortrue;
}

//----------------------------------------------------------------------------------------------------------
//##########################################################################################################
//#################################================================#########################################
//#################################                                #########################################
//#################################  SUBCLASS:    dqjacob          #########################################
//#################################                                #########################################
//#################################================================#########################################
//##########################################################################################################
//----------------------------------------------------------------------------------------------------------

/*********************************************************
##########################################################
####  SUBCLASS:   dqjacob
####  FUNCTION:   get_SVD : (int DOFs) -> void
####______________________________________________________
    * Update the svd, singValues, min_singValue, min_U_svd using the jacobian from the subclass, and the input arg.
##########################################################
**********************************************************/
void Controller_Generic::dqjacob::get_SVD(int relevant_DOFs)
{
    svd.compute(jacob, ComputeFullU);
    singValues    = svd.singularValues();
    min_singValue = singValues( relevant_DOFs - 1);
    min_U_svd     = svd.matrixU().col( relevant_DOFs - 1);
}

/*********************************************************
##########################################################
####  SUBCLASS:   dqjacob
####  FUNCTION:   get_INV : (double lambda) -> void
####______________________________________________________
    * Calculate the Jacobian Inverse using SRI (sing robust inverse) with lamda
##########################################################
**********************************************************/
void Controller_Generic::dqjacob::get_INV( double lambda )
{
    MatrixXd IDENTIDADE;
    IDENTIDADE = Controller_Generic::getIDENTMATRIX_WITH_PROPER_SIZE(task_size);
    jacobInv =  (jacob.transpose())*((  jacob*jacob.transpose()
                    + (lambda)*(IDENTIDADE)   ).inverse() );
}

void Controller_Generic::dqjacob::get_INV_left( double lambda )
{
    MatrixXd IDENTIDADE;
    IDENTIDADE = Controller_Generic::getIDENTMATRIX_WITH_PROPER_SIZE(jacob.cols());
    jacobInv =  ((  jacob.transpose()*jacob + (lambda)*(IDENTIDADE)   ).inverse() )*jacob.transpose();
}

/// * Returns the inverse of a jacob_data using SRI (sing robust inverse) with lamda
MatrixXd Controller_Generic::dqjacob::get_INV( MatrixXd jacob_data, double lambda )
{
    MatrixXd IDENTIDADE;
    MatrixXd outpumatrix;
    IDENTIDADE = Controller_Generic::getIDENTMATRIX_WITH_PROPER_SIZE( jacob_data.rows()  );
    // outpumatrix = jacob_data.transpose();
    outpumatrix = (jacob_data.transpose())*((  jacob_data*jacob_data.transpose() + (lambda)*(IDENTIDADE)   ).inverse() );
    return  outpumatrix;
}

/*********************************************************
##########################################################
####  SUBCLASS:   dqjacob
####  FUNCTION:   get_INV : (double lambda, srivar_lambda_max, srivar_lambda_region) -> void
####______________________________________________________
    * Calculate the Jacobian Inverse using SRI-VAR (sing robust inverse) with lamda, lambda_var, lambda_var_region
##########################################################
**********************************************************/
void Controller_Generic::dqjacob::get_INV(double lambda, double srivar_lamda_max, double srivar_lamda_region )
{
    double      SRI_VAR_LAMBDA;
    MatrixXd    IDENTIDADE;
    SRI_VAR_LAMBDA  = srivar_lamda_max;
    if (task_size==8)
        IDENTIDADE = Matrix<double,8,8>::Identity();
    else
        IDENTIDADE = Matrix<double,4,4>::Identity();

    if (min_singValue < srivar_lamda_region)
        SRI_VAR_LAMBDA = (  1-(min_singValue/srivar_lamda_region)*(min_singValue/srivar_lamda_region)  )*srivar_lamda_max;
    // Get JACOBIAN INVERSE:
    jacobInv =  (jacob.transpose())*((  jacob*jacob.transpose()
                    + (lambda)*(IDENTIDADE)
                    + (SRI_VAR_LAMBDA)*(min_U_svd*min_U_svd.transpose())  ).inverse() );
}
////// * Returns the inverse of a jacob_data using SRI-VAR with lamda, lambda_var, lambda_var_region

} // END OF NAMESPACE


#include "drake/solvers/csdp_solver.h"

#include <algorithm>
#include <limits>
#include <stdexcept>
#include <unordered_map>
#include <vector>

#include <Eigen/Core>
#include <Eigen/Sparse>
#include <Eigen/SparseQR>

#include "drake/common/text_logging.h"
#include "drake/solvers/csdp_solver_internal.h"

namespace drake {
namespace solvers {
namespace {
using internal::ConvertCsdpBlockMatrixtoEigen;
using internal::SdpaFreeFormat;
using internal::DecisionVariableInSdpaX;
using internal::CsdpMatrixIndex;

void SetCsdpSolverDetails(int csdp_ret, double pobj, double dobj, int y_size,
                          double* y, const csdp::blockmatrix& Z,
                          CsdpSolverDetails* solver_details) {
  solver_details->return_code = csdp_ret;
  solver_details->primal_objective = pobj;
  solver_details->dual_objective = dobj;
  solver_details->y_val.resize(y_size);
  for (int i = 0; i < y_size; ++i) {
    // CSDP uses Fortran 1-index array so the array y points to will be of size
    // y_size + 1 with the zero entry ignored.
    solver_details->y_val(i) = y[i + 1];
  }
  ConvertCsdpBlockMatrixtoEigen(Z, &(solver_details->Z_val));
}

SolutionResult ConvertCsdpReturnToSolutionResult(int csdp_ret) {
  switch (csdp_ret) {
    case 0:
    case 3:
      return SolutionResult::kSolutionFound;
    case 1:
      return SolutionResult::kInfeasibleConstraints;
    case 2:
      return SolutionResult::kDualInfeasible;
    case 4:
      return SolutionResult::kIterationLimit;
    default:
      return SolutionResult::kUnknownError;
  }
}

void SetProgramSolution(const SdpaFreeFormat& sdpa_free_format,
                        const csdp::blockmatrix& X,
                        const Eigen::VectorXd& s_val,
                        Eigen::VectorXd* prog_sol) {
  for (int i = 0;
       i < static_cast<int>(sdpa_free_format.prog_var_in_sdpa().size()); ++i) {
    if (std::holds_alternative<DecisionVariableInSdpaX>(
            sdpa_free_format.prog_var_in_sdpa()[i])) {
      const auto& decision_var_in_X = std::get<DecisionVariableInSdpaX>(
          sdpa_free_format.prog_var_in_sdpa()[i]);

      double X_entry_val{};
      switch (X.blocks[decision_var_in_X.entry_in_X.block_index + 1]
                  .blockcategory) {
        case csdp::MATRIX: {
          X_entry_val =
              X.blocks[decision_var_in_X.entry_in_X.block_index + 1]
                  .data.mat[CsdpMatrixIndex(
                      decision_var_in_X.entry_in_X.row_index_in_block,
                      decision_var_in_X.entry_in_X.column_index_in_block,
                      X.blocks[decision_var_in_X.entry_in_X.block_index + 1]
                          .blocksize)];
          break;
        }
        case csdp::DIAG: {
          X_entry_val =
              X.blocks[decision_var_in_X.entry_in_X.block_index + 1]
                  .data
                  .vec[decision_var_in_X.entry_in_X.row_index_in_block + 1];
          break;
        }
        default: {
          throw std::runtime_error(
              "SetProgramSolution(): unknown X block type.");
        }
      }
      (*prog_sol)(i) =
          decision_var_in_X.offset +
          (decision_var_in_X.coeff_sign == internal::Sign::kPositive
               ? X_entry_val
               : -X_entry_val);
    } else if (std::holds_alternative<double>(
                   sdpa_free_format.prog_var_in_sdpa()[i])) {
      (*prog_sol)(i) = std::get<double>(sdpa_free_format.prog_var_in_sdpa()[i]);
    } else if (std::holds_alternative<SdpaFreeFormat::FreeVariableIndex>(
                   sdpa_free_format.prog_var_in_sdpa()[i])) {
      (*prog_sol)(i) = s_val(std::get<SdpaFreeFormat::FreeVariableIndex>(
          sdpa_free_format.prog_var_in_sdpa()[i]));
    }
  }
}

void SolveProgramWithNoFreeVariables(
    const MathematicalProgram& prog,
    const SdpaFreeFormat& sdpa_free_format,
    MathematicalProgramResult* result) {
  DRAKE_ASSERT(sdpa_free_format.num_free_variables() == 0);

  csdp::blockmatrix C;
  double* rhs;
  csdp::constraintmatrix* constraints{nullptr};
  GenerateCsdpProblemDataWithoutFreeVariables(sdpa_free_format, &C, &rhs,
                                              &constraints);

  struct csdp::blockmatrix X, Z;
  double* y;
  csdp::initsoln(sdpa_free_format.num_X_rows(), sdpa_free_format.g().rows(), C,
                 rhs, constraints, &X, &y, &Z);
  double pobj, dobj;
  const int ret = csdp::easy_sdp(
      sdpa_free_format.num_X_rows(), sdpa_free_format.g().rows(), C, rhs,
      constraints, -sdpa_free_format.constant_min_cost_term(), &X, &y, &Z,
      &pobj, &dobj);

  // Set solver details.
  CsdpSolverDetails& solver_details =
      result->SetSolverDetailsType<CsdpSolverDetails>();
  SetCsdpSolverDetails(ret, pobj, dobj, sdpa_free_format.g().rows(), y, Z,
                       &solver_details);
  // Retrieve the information back to result.
  // Since CSDP solves a maximization problem max -cost, where "cost" is the
  // minimization cost in MathematicalProgram, we need to negate the cost.
  result->set_optimal_cost(
      ret == 1 /* primal infeasible */ ? MathematicalProgram::
                                             kGlobalInfeasibleCost
                                       : -pobj);
  result->set_solution_result(ConvertCsdpReturnToSolutionResult(ret));
  Eigen::VectorXd prog_sol(prog.num_vars());
  SetProgramSolution(sdpa_free_format, X, Eigen::VectorXd::Zero(0), &prog_sol);
  result->set_x_val(prog_sol);

  csdp::free_prob(sdpa_free_format.num_X_rows(), sdpa_free_format.g().rows(), C,
                  rhs, constraints, X, y, Z);
}

void SolveProgramThroughNullspaceApproach(
    const MathematicalProgram& prog, const SdpaFreeFormat& sdpa_free_format,
    MathematicalProgramResult* result) {
  static const logging::Warn log_once(
      "The problem has free variables, and CSDP removes the free "
      "variables by computing the null space of linear constraint in the "
      "dual space. This step can be time consuming. Consider providing a lower "
      "and/or upper bound for each decision variable.");
  Eigen::SparseMatrix<double> C_hat;
  std::vector<Eigen::SparseMatrix<double>> A_hat;
  Eigen::VectorXd rhs_hat, y_hat;
  Eigen::SparseQR<Eigen::SparseMatrix<double>, Eigen::COLAMDOrdering<int>> QR_B;
  sdpa_free_format.RemoveFreeVariableByNullspaceApproach(
      &C_hat, &A_hat, &rhs_hat, &y_hat, &QR_B);

  // Now try to call CSDP to solve this problem.
  csdp::blockmatrix C_csdp;
  double* rhs_csdp{nullptr};
  csdp::constraintmatrix* constraints_csdp{nullptr};
  ConvertSparseMatrixFormatToCsdpProblemData(sdpa_free_format.X_blocks(),
                                              C_hat, A_hat, rhs_hat, &C_csdp,
                                              &rhs_csdp, &constraints_csdp);
  struct csdp::blockmatrix X_csdp, Z;
  double* y{nullptr};
  csdp::initsoln(sdpa_free_format.num_X_rows(), rhs_hat.rows(), C_csdp,
                 rhs_csdp, constraints_csdp, &X_csdp, &y, &Z);
  double pobj{0};
  double dobj{0};
  const int ret = csdp::easy_sdp(sdpa_free_format.num_X_rows(), rhs_hat.rows(),
                                 C_csdp, rhs_csdp, constraints_csdp,
                                 -sdpa_free_format.constant_min_cost_term() +
                                     sdpa_free_format.g().dot(y_hat),
                                 &X_csdp, &y, &Z, &pobj, &dobj);
  Eigen::SparseMatrix<double> X_hat(sdpa_free_format.num_X_rows(),
                                    sdpa_free_format.num_X_rows());
  ConvertCsdpBlockMatrixtoEigen(X_csdp, &X_hat);
  // Now compute the free variable values.
  // AX(i) is trace(Ai, X_hat)
  Eigen::VectorXd AX(sdpa_free_format.A().size());
  for (int i = 0; i < AX.rows(); ++i) {
    AX(i) = (sdpa_free_format.A()[i].cwiseProduct(X_hat)).sum();
  }
  Eigen::VectorXd s_val;
  s_val = QR_B.solve(sdpa_free_format.g() - AX);
  // Set solver details.
  CsdpSolverDetails& solver_details =
      result->SetSolverDetailsType<CsdpSolverDetails>();
  SetCsdpSolverDetails(ret, pobj, dobj, rhs_hat.rows(), y, Z,
                       &solver_details);
  // Retrieve the information back to result
  // Since CSDP solves a maximization problem max -cost, where "cost" is the
  // minimization cost in MathematicalProgram, we need to negate the cost.
  result->set_solution_result(ConvertCsdpReturnToSolutionResult(ret));
  result->set_optimal_cost(
      ret == 1 /* primal infeasible */ ? MathematicalProgram::
                                             kGlobalInfeasibleCost
                                       : -pobj);
  Eigen::VectorXd prog_sol(prog.num_vars());
  SetProgramSolution(sdpa_free_format, X_csdp, s_val, &prog_sol);
  result->set_x_val(prog_sol);

  csdp::free_prob(sdpa_free_format.num_X_rows(), rhs_hat.rows(), C_csdp,
                  rhs_csdp, constraints_csdp, X_csdp, y, Z);
}

/**
 * For the problem
 * max tr(C * X) + dᵀs
 * s.t tr(Aᵢ*X) + bᵢᵀs = aᵢ
 *     X ≽ 0
 *     s is free.
 * Remove the free variable s by introducing two slack variables y⁺ ≥ 0 and y⁻
 * ≥ 0, and the constraint y⁺ - y⁻ = s. We get a new program without free
 * variables.
 * max tr(Ĉ * X̂)
 * s.t tr(Âᵢ*X̂) = aᵢ
 *     X̂ ≽ 0
 * where Ĉ = diag(C, diag(d), -diag(d))
 *       X̂ = diag(X, diag(y⁺), diag(y⁻))
 *       Âᵢ = diag(Aᵢ, diag(bᵢ), -diag(bᵢ))
 */
void SolveProgramThroughTwoSlackVariablesApproach(
    const MathematicalProgram& prog, const SdpaFreeFormat& sdpa_free_format,
    MathematicalProgramResult* result) {
  static const logging::Warn log_once(
      "The problem has free variables, and CSDP removes the free "
      "variables by introducing the slack variable y_plus >=0 , y_minus >= "
      "0, and constraint y_plus - y_minus = free_variable. This can "
      "introduce numerical problems to the solver. Consider providing a lower "
      "and/or upper bound for each decision variable.");
  std::vector<internal::BlockInX> X_hat_blocks;
  std::vector<Eigen::SparseMatrix<double>> A_hat;
  Eigen::SparseMatrix<double> C_hat;
  sdpa_free_format.RemoveFreeVariableByTwoSlackVariablesApproach(
      &X_hat_blocks, &A_hat, &C_hat);
  const int num_X_hat_rows =
      sdpa_free_format.num_X_rows() + 2 * sdpa_free_format.num_free_variables();

  // Now try to call CSDP to solve this problem.
  csdp::blockmatrix C_csdp;
  double* rhs_csdp{nullptr};
  csdp::constraintmatrix* constraints_csdp{nullptr};
  ConvertSparseMatrixFormatToCsdpProblemData(X_hat_blocks, C_hat, A_hat,
                                              sdpa_free_format.g(), &C_csdp,
                                              &rhs_csdp, &constraints_csdp);
  struct csdp::blockmatrix X_csdp, Z;
  double* y{nullptr};
  csdp::initsoln(num_X_hat_rows, sdpa_free_format.g().rows(), C_csdp, rhs_csdp,
                 constraints_csdp, &X_csdp, &y, &Z);
  double pobj{0};
  double dobj{0};
  const int ret = csdp::easy_sdp(num_X_hat_rows, sdpa_free_format.g().rows(),
                                 C_csdp, rhs_csdp, constraints_csdp,
                                 -sdpa_free_format.constant_min_cost_term(),
                                 &X_csdp, &y, &Z, &pobj, &dobj);
  Eigen::SparseMatrix<double> X_hat(num_X_hat_rows, num_X_hat_rows);
  ConvertCsdpBlockMatrixtoEigen(X_csdp, &X_hat);
  // Now retrieve the value for the free variable s as y⁺ - y⁻.
  Eigen::VectorXd s_val(sdpa_free_format.num_free_variables());
  for (int i = 0; i < sdpa_free_format.num_free_variables(); ++i) {
    double y_plus_val{0};
    for (Eigen::SparseMatrix<double>::InnerIterator it(
             X_hat, sdpa_free_format.num_X_rows() + i);
         it; ++it) {
      y_plus_val = it.value();
    }
    double y_minus_val{0};
    for (Eigen::SparseMatrix<double>::InnerIterator it(
             X_hat, sdpa_free_format.num_X_rows() +
                        sdpa_free_format.num_free_variables() + i);
         it; ++it) {
      y_minus_val = it.value();
    }
    s_val(i) = y_plus_val - y_minus_val;
  }

  CsdpSolverDetails& solver_details =
      result->SetSolverDetailsType<CsdpSolverDetails>();
  SetCsdpSolverDetails(ret, pobj, dobj, sdpa_free_format.g().rows(), y, Z,
                       &solver_details);
  // Retrieve the information back to result
  // Since CSDP solves a maximization problem max -cost, where "cost" is the
  // minimization cost in MathematicalProgram, we need to negate the cost.
  result->set_solution_result(ConvertCsdpReturnToSolutionResult(ret));
  result->set_optimal_cost(
      ret == 1 /* primal infeasible */ ? MathematicalProgram::
                                             kGlobalInfeasibleCost
                                       : -pobj);
  Eigen::VectorXd prog_sol(prog.num_vars());
  SetProgramSolution(sdpa_free_format, X_csdp, s_val, &prog_sol);
  result->set_x_val(prog_sol);

  csdp::free_prob(num_X_hat_rows, sdpa_free_format.g().rows(), C_csdp, rhs_csdp,
                  constraints_csdp, X_csdp, y, Z);
}

/*
 * For the problem
 * max tr(C * X) + dᵀs
 * s.t tr(Aᵢ*X) + bᵢᵀs = aᵢ
 *     X ≽ 0
 *     s is free.
 * Remove the free variable s by introducing a slack variable t with the
 * Lorentz cone constraint t ≥ sqrt(sᵀs). We get a new program without free
 * variables.
 * max tr(Ĉ * X̂)
 * s.t tr(Âᵢ*X̂) = aᵢ
 *     X̂ ≽ 0
 * Refer to RemoveFreeVariableByLorentzConeSlackApproach in sdpa_free_format.h
 * for more details.
 */
void SolveProgramThroughLorentzConeSlackApproach(
    const MathematicalProgram& prog, const SdpaFreeFormat& sdpa_free_format,
    MathematicalProgramResult* result) {
  static const logging::Warn log_once(
      "The problem has free variables, and CSDP removes the free "
      "variables by introducing a slack variable t with the Lorentz cone "
      "constraint t>= sqrt(s'*s) This can introduce numerical problems to the "
      "solver. Consider providing a lower "
      "and/or upper bound for each decision variable.");
  std::vector<internal::BlockInX> X_hat_blocks;
  std::vector<Eigen::SparseMatrix<double>> A_hat;
  Eigen::VectorXd rhs_hat;
  Eigen::SparseMatrix<double> C_hat;
  sdpa_free_format.RemoveFreeVariableByLorentzConeSlackApproach(
      &X_hat_blocks, &A_hat, &rhs_hat, &C_hat);
  const int num_X_hat_rows =
      sdpa_free_format.num_X_rows() + sdpa_free_format.num_free_variables() + 1;

  // Now try to call CSDP to solve this problem.
  csdp::blockmatrix C_csdp;
  double* rhs_csdp{nullptr};
  csdp::constraintmatrix* constraints_csdp{nullptr};
  ConvertSparseMatrixFormatToCsdpProblemData(X_hat_blocks, C_hat, A_hat,
                                             rhs_hat, &C_csdp, &rhs_csdp,
                                             &constraints_csdp);
  struct csdp::blockmatrix X_csdp, Z;
  double* y{nullptr};
  csdp::initsoln(num_X_hat_rows, rhs_hat.rows(), C_csdp, rhs_csdp,
                 constraints_csdp, &X_csdp, &y, &Z);
  double pobj{0};
  double dobj{0};
  const int ret = csdp::easy_sdp(num_X_hat_rows, rhs_hat.rows(), C_csdp,
                                 rhs_csdp, constraints_csdp,
                                 -sdpa_free_format.constant_min_cost_term(),
                                 &X_csdp, &y, &Z, &pobj, &dobj);
  Eigen::SparseMatrix<double> X_hat(num_X_hat_rows, num_X_hat_rows);
  ConvertCsdpBlockMatrixtoEigen(X_csdp, &X_hat);
  // Now retrieve the value for the free variable s from Y.
  Eigen::VectorXd s_val =
      Eigen::VectorXd::Zero(sdpa_free_format.num_free_variables());
  for (int i = 0; i < sdpa_free_format.num_free_variables(); ++i) {
    for (Eigen::SparseMatrix<double>::InnerIterator it(
             X_hat, sdpa_free_format.num_X_rows() + i + 1);
         it; ++it) {
      s_val(i) = it.value();
      // There are two non-zero entries in this column, Y(0, i+1) = s(i) and
      // Y(i+1, i+1) = t(i). We only care about the first non-zero entry Y(0,
      // i+1), so break here.
      break;
    }
  }

  CsdpSolverDetails& solver_details =
      result->SetSolverDetailsType<CsdpSolverDetails>();
  SetCsdpSolverDetails(ret, pobj, dobj, rhs_hat.rows(), y, Z, &solver_details);
  // Retrieve the information back to result.
  // Since CSDP solves a maximization problem max -cost, where "cost" is the
  // minimization cost in MathematicalProgram, we need to negate the cost.
  result->set_solution_result(ConvertCsdpReturnToSolutionResult(ret));
  result->set_optimal_cost(
      ret == 1 /* primal infeasible */ ? MathematicalProgram::
                                             kGlobalInfeasibleCost
                                       : -pobj);
  Eigen::VectorXd prog_sol(prog.num_vars());
  SetProgramSolution(sdpa_free_format, X_csdp, s_val, &prog_sol);
  result->set_x_val(prog_sol);

  csdp::free_prob(num_X_hat_rows, rhs_hat.rows(), C_csdp, rhs_csdp,
                  constraints_csdp, X_csdp, y, Z);
}
}  // namespace

void CsdpSolver::DoSolve(const MathematicalProgram& prog,
                         const Eigen::VectorXd&, const SolverOptions&,
                         MathematicalProgramResult* result) const {
  if (!prog.GetVariableScaling().empty()) {
    static const logging::Warn log_once(
        "CsdpSolver doesn't support the feature of variable scaling.");
  }

  result->set_solver_id(CsdpSolver::id());
  const SdpaFreeFormat sdpa_free_format(prog);
  if (sdpa_free_format.num_free_variables() == 0) {
    SolveProgramWithNoFreeVariables(prog, sdpa_free_format, result);
  } else {
    switch (method_) {
      case RemoveFreeVariableMethod::kNullspace: {
        SolveProgramThroughNullspaceApproach(prog, sdpa_free_format, result);
        break;
      }
      case RemoveFreeVariableMethod::kTwoSlackVariables: {
        SolveProgramThroughTwoSlackVariablesApproach(prog, sdpa_free_format,
                                                     result);
        break;
      }
      case RemoveFreeVariableMethod::kLorentzConeSlack: {
        SolveProgramThroughLorentzConeSlackApproach(prog, sdpa_free_format,
                                                    result);
        break;
      }
    }
  }
}

bool CsdpSolver::is_available() { return true; }
}  // namespace solvers
}  // namespace drake


/*
  [auto_generated]
  boost/numeric/odeint/external/blaze/blaze_algebra_dispatcher.hpp

  [begin_description]
  tba.
  [end_description]

  Copyright 2009-2012 Karsten Ahnert
  Copyright 2009-2012 Mario Mulansky

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

#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_BLAZE_BLAZE_ALGEBRA_DISPATCHER_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_BLAZE_BLAZE_ALGEBRA_DISPATCHER_HPP_INCLUDED

#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <blaze/math/dense/StaticVector.h>
#include <blaze/math/dense/DynamicVector.h>

namespace boost {
namespace numeric {
namespace odeint {

template< typename T , size_t N , bool TF >
struct algebra_dispatcher< blaze::StaticVector< T , N , TF >  >
{
    typedef vector_space_algebra algebra_type;
};

template< typename T , bool TF >
struct algebra_dispatcher< blaze::DynamicVector< T , TF > >
{
    typedef vector_space_algebra algebra_type;
};

}
}
}

#endif // BOOST_NUMERIC_ODEINT_EXTERNAL_BLAZE_BLAZE_ALGEBRA_DISPATCHER_HPP_INCLUDED


#include <algorithm>
#include <cmath>
#include <cstring>
#include <fstream>
#include <sstream>
#include <iostream>
#include <iterator>
#include <string>
#include <vector>
#include <map>
#include <boost/dynamic_bitset.hpp>
#include <boost/program_options.hpp>

namespace po = boost::program_options;

using namespace std;
using namespace boost;

class Corpus {
public:
    string text;
    dynamic_bitset<> utt_boundaries;
    dynamic_bitset<> boundaries;
    map<char, double> p_phonemes;

    Corpus(string path)
    {
        load_file({path});
        compute_p_phonemes();
    }

    Corpus(string path, string trained_path,
           dynamic_bitset<> &trained_boundaries)
    {
        load_file({path, trained_path});
        compute_p_phonemes();

        // set the trained boundaries as unchangable utterance boundaries
        trained_boundaries.resize(text.size() + 1, false);
        utt_boundaries |= trained_boundaries;
    }

    double p0(string word, double p_hash=0.5)
    {
        double p = 1;
        for (char ph : word)
            p *= p_phonemes[ph];

        return p_hash * pow(1 - p_hash, word.size() - 1) * p;
    }

    vector<string> get_words()
    {
        vector<string> words;
        dynamic_bitset<> bounds = utt_boundaries | boundaries;

        for (int i = 0; i < text.size(); ++i) {
            if (bounds[i])
                words.push_back(string());

            words.back().append(1, text[i]);
        }

        return words;
    }

private:
    virtual void load_file(const vector<string> &paths)
    {
        string line;
        stringstream text_stream;
        vector<int> utt_indices = {0};

        for (string path : paths) {
            fstream in(path);

            while (getline(in, line)) {
                text_stream << line;
                utt_indices.push_back(utt_indices.back() + line.size());
            }

            in.close();
        }

        text = text_stream.str();

        utt_boundaries = dynamic_bitset<>(text.size() + 1);
        boundaries = dynamic_bitset<>(text.size() + 1);

        for (int index : utt_indices) {
            utt_boundaries[index] = 1;
        }
    }

    void compute_p_phonemes()
    {
        for (char phoneme : text) {
            // only compute if the char isn't in the map yet
            if (p_phonemes.find(phoneme) == p_phonemes.end())
                p_phonemes[phoneme] = count(text.begin(), text.end(), phoneme) /
                    (double)text.size();
        }
    }
};

map<string, int> histogram(const vector<string> &vec)
{
    map<string, int> counts;
    vector<string> unique_words;

    unique_copy(vec.begin(), vec.end(), back_inserter(unique_words));

    for (const string &word : unique_words) {
        if (counts.find(word) == counts.end())
            counts[word] = 1;
        else
            counts[word] += 1;
    }

    return counts;
}

void find_enclosing_boundaries(dynamic_bitset<> bounds, int i,
                               int *lower, int *upper)
{
    *lower = i - 1;
    while (!bounds[*lower])
        *lower -= 1;

    *upper = i + 1;
    while (!bounds[*upper])
        *upper += 1;
}

void gibbs_iteration(Corpus &corpus, double rho=2.0, double alpha=0.5,
                     double p_hash=0.5)
{
    dynamic_bitset<> bounds = corpus.utt_boundaries | corpus.boundaries;
    vector<string> words = corpus.get_words();
    map<string, int> word_counts = histogram(words);

    for (int i = 0; i < corpus.text.size(); ++i) {
        char phoneme = corpus.text[i];

        if (corpus.utt_boundaries[i])
            continue;

        int lower, upper;
        find_enclosing_boundaries(bounds, i, &lower, &upper);

        string w1 = corpus.text.substr(lower, (upper - lower));
        string w2 = corpus.text.substr(lower, (i - lower));
        string w3 = corpus.text.substr(i, (upper - i));

        double n_;
        if (!bounds[i])
            n_ = words.size();
        else
            n_ = words.size() - 1;

        double n_dollar = corpus.utt_boundaries.count() - 1;
        double nu = corpus.utt_boundaries[upper] ? n_dollar : n_ - n_dollar;

        double p_h1_factor1;
        if (!bounds[i])
            p_h1_factor1 = (word_counts[w1] - 1 + alpha * corpus.p0(w1, p_hash)) / (n_ + alpha);
        else
            p_h1_factor1 = (word_counts[w1] + alpha * corpus.p0(w1, p_hash)) / (n_ + alpha);

        double p_h1_factor2 = (nu + rho/2) / (n_ + rho);

        double p_h2_factor1, p_h2_factor3;
        if (!bounds[i]) {
            p_h2_factor1 = (word_counts[w2] + alpha * corpus.p0(w2, p_hash)) / (n_ + alpha);
            p_h2_factor3 = ((word_counts[w3] + (w2 == w3 ? 1 : 0) + alpha *
                             corpus.p0(w3, p_hash)) / (n_ + 1 + alpha));
        } else {
            p_h2_factor1 = (word_counts[w2] - 1 + alpha * corpus.p0(w2, p_hash)) / (n_ + alpha);
            p_h2_factor3 = ((word_counts[w3] - 1 + (w2 == w3 ? 1 : 0) + alpha *
                             corpus.p0(w3, p_hash)) / (n_ + 1 + alpha));
        }

        double p_h2_factor2 = (n_ - n_dollar + rho/2) / (n_ + rho);
        double p_h2_factor4 = ((nu + (w2 == w3 ? 1 : 0) + rho/2) / (n_ + 1 + rho));

        double p_h1 = p_h1_factor1 * p_h1_factor2;
        double p_h2 = p_h2_factor1 * p_h2_factor2 * p_h2_factor3 * p_h2_factor4;

        if (p_h2 > p_h1)
            corpus.boundaries[i] = 1;
        else
            corpus.boundaries[i] = 0;
    }
}

void write_boundaries(const Corpus &corpus, string filename)
{
    ofstream out(filename);
    out << "[";

    for (int i = 0; i < corpus.text.size(); ++i) {
        if (i != 0)
            out << ", ";
        out << corpus.boundaries[i];
    }

    out << "]";
    out.close();
}

dynamic_bitset<> read_boundaries(string path)
{
    fstream in(path);
    string line;
    getline(in, line);
    in.close();

    auto end_iter = remove_if(line.begin(), line.end(),
                              [](char ch) {return !(ch == '1' || ch == '0');});
    line.erase(end_iter, line.end());

    return dynamic_bitset<>(line);
}

string generate_filepath(const string &out_dir, double alpha, double p_hash, int i=-1)
{
    stringstream s;
    s << out_dir << "/";
    if (i >= 0)
        s << "iter_" << i << "_";
    s << alpha << "_" << p_hash << ".txt";

    return s.str();
}

int main(int argc, char *argv[])
{
    string out_dir;
    string train_path;
    string test_path;
    string boundaries;
    double alpha;
    double p_hash;
    vector<int> eval_points;

    po::options_description desc("Usage");
    desc.add_options()
        ("alpha,a", po::value<double>(&alpha)->default_value(0.5), "alpha parameter")
        ("p_hash,ph", po::value<double>(&p_hash)->default_value(0.5), "p# parameter")
        ("out_dir", po::value<string>(&out_dir), "path to write output to")
        ("train_path", po::value<string>(&train_path), "path to load training data")
        ("test_path", po::value<string>(&test_path), "path to load test data")
        ("test", "apply a learned model to test data (requires test_path and boundaries)")
        ("help", "print usage information")
        ("boundaries", po::value<string>(&boundaries), "path to a learned set of boundaries")
        (",n", po::value<vector<int> >(&eval_points)->multitoken(), "points to evaluate");

    po::variables_map opts;
    po::store(po::parse_command_line(argc, argv, desc), opts);
    po::notify(opts);

    if (opts.count("help")) {
        cout << desc << endl;
        return 0;
    }

    string out_path = generate_filepath(out_dir, alpha, p_hash);

    Corpus *corpus;
    if (opts.count("test")) {
        cout << "Testing" << endl;
        auto bounds = read_boundaries(boundaries);
        corpus = new Corpus(train_path, test_path, bounds);
    } else {
        corpus = new Corpus(train_path);
    }

    // calculate the maximum iteration
    int i_max = *max_element(eval_points.begin(), eval_points.end());

    for (int i = 0; i < i_max; ++i) {
        cout << "Iteration " << i << endl;
        gibbs_iteration(*corpus, 2, alpha, p_hash);

        if (find(eval_points.begin(), eval_points.end(), i) != eval_points.end()) {
            string iterpath = generate_filepath(out_dir, alpha, p_hash, i);
            write_boundaries(*corpus, iterpath);
        }
    }

    write_boundaries(*corpus, out_path);

    return 0;
}


#include <tradelayer/consensushash.h>
#include <tradelayer/errors.h>
#include <tradelayer/externfns.h>
#include <tradelayer/log.h>
#include <tradelayer/mdex.h>
#include <tradelayer/rules.h>
#include <tradelayer/sp.h>
#include <tradelayer/tally.h>
#include <tradelayer/tradelayer.h>
#include <tradelayer/tx.h>
#include <tradelayer/uint256_extensions.h>

#include <arith_uint256.h>
#include <chain.h>
#include <test/test_bitcoin.h>
#include <tinyformat.h>
#include <uint256.h>
#include <util/strencodings.h>

#include <univalue.h>

#include <assert.h>
#include <fstream>
#include <limits>
#include <map>
#include <set>
#include <stdint.h>
#include <string>
#include <vector>

#include <boost/multiprecision/cpp_int.hpp>
#include <boost/rational.hpp>
#include <boost/test/unit_test.hpp>

using namespace mastercore;

BOOST_FIXTURE_TEST_SUITE(vesting_tests, BasicTestingSetup)

/** Proof of concept function **/
int VestingTokens(double& lastVesting, int64_t globalVolumeALL_LTC, int64_t& nAmount, std::vector<std::string>& vestingAddresses)
{
    bool deactivation = false;

    if (vestingAddresses.empty()){
        return -1;
    }

    if (globalVolumeALL_LTC <= 10000 * COIN){
        return -2;
    }

    if(100000000 * COIN <= globalVolumeALL_LTC){
        deactivation = true;
    }

    // accumulated vesting
    const double accumVesting = getAccumVesting(globalVolumeALL_LTC);

    // vesting fraction on this block
    const double realVesting = (accumVesting > lastVesting) ?  accumVesting - lastVesting : 0;

    if (realVesting == 0){
        return -4;
    }

    for (auto &addr : vestingAddresses)
    {

        const int64_t vestingBalance = getMPbalance(addr, TL_PROPERTY_VESTING, BALANCE);
        const int64_t unvestedALLBal = getMPbalance(addr, ALL, UNVESTED);

        if (vestingBalance != 0 && unvestedALLBal != 0 && globalVolumeALL_LTC != 0)
        {

            const int64_t iRealVesting = mastercore::DoubleToInt64(realVesting);
            const arith_uint256 uVestingBalance = mastercore::ConvertTo256(vestingBalance);
            const arith_uint256 uCOIN  = mastercore::ConvertTo256(COIN);
            const arith_uint256 iAmount = mastercore::ConvertTo256(iRealVesting) * DivideAndRoundUp(uVestingBalance, uCOIN);
            nAmount = mastercore::ConvertTo64(iAmount);

            if(deactivation){
                BOOST_TEST_MESSAGE("Passing ALL left unvested to balance");
                nAmount = unvestedALLBal;
            }

            // BOOST_TEST_MESSAGE("vestingBalance" << vestingBalance << "\n");
            // BOOST_TEST_MESSAGE("unvestedALLBal" << unvestedALLBal);
            // BOOST_TEST_MESSAGE("nAmount" << nAmount);
            // BOOST_TEST_MESSAGE("iRealVesting" << iRealVesting);
            assert(update_tally_map(addr, ALL, -nAmount, UNVESTED));
            assert(update_tally_map(addr, ALL, nAmount, BALANCE));
        }
    }

    lastVesting = accumVesting;

    return 0;
}

BOOST_AUTO_TEST_CASE(empty_address)
{
    int64_t nAmount = 0;
    double lastVesting = 0;
    std::vector<std::string> vestingAddresses;
    BOOST_CHECK_EQUAL(-1, VestingTokens(lastVesting, 0, nAmount, vestingAddresses));

}

BOOST_AUTO_TEST_CASE(out_of_range_volume)
{
    int64_t nAmount = 0;
    double lastVesting = 0;
    std::vector<std::string> vestingAddresses;
    // adding first vesting address
    vestingAddresses.push_back("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPb");

    // putting 1000 vesting tokens
    BOOST_CHECK(mastercore::update_tally_map("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPb", TL_PROPERTY_VESTING, 1000 * COIN, BALANCE));
    BOOST_CHECK(mastercore::update_tally_map("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPb", ALL, 1000 * COIN, UNVESTED));
    //checking ALL balance is zero
    BOOST_CHECK_EQUAL(0, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPb", ALL, BALANCE));

    BOOST_CHECK_EQUAL(-2, VestingTokens(lastVesting, 0, nAmount, vestingAddresses));
    BOOST_CHECK_EQUAL(-2, VestingTokens(lastVesting, COIN, nAmount, vestingAddresses));
    BOOST_CHECK_EQUAL(-2, VestingTokens(lastVesting, 10 * COIN, nAmount, vestingAddresses));
    BOOST_CHECK_EQUAL(-2, VestingTokens(lastVesting, 100 * COIN, nAmount, vestingAddresses));
    BOOST_CHECK_EQUAL(-2, VestingTokens(lastVesting, 1000 * COIN, nAmount, vestingAddresses));
    BOOST_CHECK_EQUAL(-2, VestingTokens(lastVesting, 10000 * COIN, nAmount, vestingAddresses));

    // last vesting batch
    BOOST_CHECK_EQUAL(0, VestingTokens(lastVesting, 100000010 * COIN, nAmount, vestingAddresses));

    // amount bigger than max volume
    BOOST_CHECK_EQUAL(-4, VestingTokens(lastVesting, 100000100 * COIN, nAmount, vestingAddresses));
    BOOST_CHECK_EQUAL(-4, VestingTokens(lastVesting, 100001000 * COIN, nAmount, vestingAddresses));
    BOOST_CHECK_EQUAL(-4, VestingTokens(lastVesting, 100010000 * COIN, nAmount, vestingAddresses));
    BOOST_CHECK_EQUAL(-4, VestingTokens(lastVesting, 100100000 * COIN, nAmount, vestingAddresses));

    // must have entire volume
    BOOST_CHECK_EQUAL(100000000000, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPb", ALL, BALANCE));
}

BOOST_AUTO_TEST_CASE(vesting_process)
{
  int64_t nAmount = 0;
  double lastVesting = 0;
  std::vector<std::string> vestingAddresses;
  // adding first vesting address
  vestingAddresses.push_back("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx");
  // putting 1000 vesting tokens
  BOOST_CHECK(mastercore::update_tally_map("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", TL_PROPERTY_VESTING, 1000 * COIN, BALANCE));
  BOOST_CHECK(mastercore::update_tally_map("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, 1000 * COIN, UNVESTED));
  //checking ALL balance is zero
  BOOST_CHECK_EQUAL(0, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, BALANCE));
  // checking vestingAddresses size
  BOOST_CHECK_EQUAL(1, vestingAddresses.size());

  // checking 7.52% vesting output
  BOOST_CHECK_EQUAL(0, VestingTokens(lastVesting, 20000 * COIN, nAmount, vestingAddresses));
  //checking ALL balance
  BOOST_CHECK_EQUAL(7525749000, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, BALANCE));
  //checking unvested ALL balance
  BOOST_CHECK_EQUAL(92474251000, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, UNVESTED));
  // all unvested + balance = 1000
  BOOST_CHECK_EQUAL(1000 * COIN, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, UNVESTED) + getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, BALANCE));
  // lastVesting: 7.52%
  BOOST_CHECK_EQUAL(0.075257498915995313, lastVesting);
  // amount vested in this block (7.52%)
  BOOST_CHECK_EQUAL(7525749000, nAmount);

  // checking 15.05% vesting output
  BOOST_CHECK_EQUAL(0, VestingTokens(lastVesting, 40000 * COIN, nAmount, vestingAddresses));
  //checking ALL balance
  BOOST_CHECK_EQUAL(15051498000, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, BALANCE));
  // //checking unvested ALL balance
  BOOST_CHECK_EQUAL(84948502000, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, UNVESTED));
  // all unvested + balance = 1000
  BOOST_CHECK_EQUAL(1000 * COIN, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, UNVESTED) + getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, BALANCE));
  // lastVesting: 15.05%
  BOOST_CHECK_EQUAL(0.15051499783199063, lastVesting);
  // amount vested in this block (7.53%)
  BOOST_CHECK_EQUAL(7525749000, nAmount);

  // checking 25% vesting output
  BOOST_CHECK_EQUAL(0, VestingTokens(lastVesting, 100000 * COIN, nAmount, vestingAddresses));
  //checking ALL balance
  BOOST_CHECK_EQUAL(24999998000, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, BALANCE));
  // //checking unvested ALL balance
  BOOST_CHECK_EQUAL(75000002000, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, UNVESTED));
  // all unvested + balance = 1000
  BOOST_CHECK_EQUAL(1000 * COIN, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, UNVESTED) + getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, BALANCE));
  // lastVesting: 25%
  BOOST_CHECK_EQUAL(0.25000000000000000, lastVesting);
  // amount vested in this block (9.95%)
  BOOST_CHECK_EQUAL(9948500000, nAmount);

  // checking 50% vesting output
  BOOST_CHECK_EQUAL(0, VestingTokens(lastVesting, 1000000 * COIN, nAmount, vestingAddresses));
  //checking ALL balance
  BOOST_CHECK_EQUAL(49999998000, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, BALANCE));
  // //checking unvested ALL balance
  BOOST_CHECK_EQUAL(50000002000, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, UNVESTED));
  // all unvested + balance = 1000
  BOOST_CHECK_EQUAL(1000 * COIN, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, UNVESTED) + getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, BALANCE));
  // lastVesting: 50%
  BOOST_CHECK_EQUAL(0.50000000000000000, lastVesting);
  // amount vested in this block (25%)
  BOOST_CHECK_EQUAL(25000000000, nAmount);

  // checking 75% vesting output
  BOOST_CHECK_EQUAL(0, VestingTokens(lastVesting, 10000000 * COIN, nAmount, vestingAddresses));
  //checking ALL balance
  BOOST_CHECK_EQUAL(74999998000, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, BALANCE));
  // //checking unvested ALL balance
  BOOST_CHECK_EQUAL(25000002000, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, UNVESTED));
  // all unvested + balance = 1000
  BOOST_CHECK_EQUAL(1000 * COIN, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, UNVESTED) + getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, BALANCE));
  // lastVesting: 75%
  BOOST_CHECK_EQUAL(0.75000000000000000, lastVesting);
  // amount vested in this block (25%)
  BOOST_CHECK_EQUAL(25000000000, nAmount);

  // checking 100% vesting output
  BOOST_CHECK_EQUAL(0, VestingTokens(lastVesting, 1000000000 * COIN, nAmount, vestingAddresses));
  // checking ALL balance
  BOOST_CHECK_EQUAL(100000000000, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, BALANCE));
  // //checking unvested ALL balance
  BOOST_CHECK_EQUAL(0, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, UNVESTED));
  // all unvested + balance = 1000
  BOOST_CHECK_EQUAL(1000 * COIN, getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, UNVESTED) + getMPbalance("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, BALANCE));
  // lastVesting: 75%
  BOOST_CHECK_EQUAL(1, lastVesting);
  // amount vested in this block (25%)
  BOOST_CHECK_EQUAL(25000002000, nAmount);

}

BOOST_AUTO_TEST_CASE(simple_calculation)
{
    double lastVesting = 0;
    const int64_t LTC_Amount = 2807759143600;
    int64_t nAmount = 0;
    std::vector<std::string> vestingAddresses;
    // adding first vesting address
    vestingAddresses.push_back("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx");

    // putting 1276 vesting tokens
    BOOST_CHECK(mastercore::update_tally_map("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", TL_PROPERTY_VESTING, 127600000000, BALANCE));
    BOOST_CHECK(mastercore::update_tally_map("QgKxFUBgR8y4xFy3s9ybpbDvYNKr4HTKPx", ALL, 127600000000, UNVESTED));

    BOOST_CHECK_EQUAL(0, VestingTokens(lastVesting, LTC_Amount, nAmount, vestingAddresses));

    BOOST_CHECK_EQUAL(0, nAmount);

}
BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  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_COMMON_SIMD_FUNCTION_MINUS_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_MINUS_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/function/bitwise_xor.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/if_else_zero.hpp>
#include <boost/simd/function/is_less_equal.hpp>
#include <boost/simd/function/is_ltz.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/shr.hpp>
#include <boost/simd/function/splat.hpp>
#include <boost/simd/detail/dispatch/meta/scalar_of.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD_IF(minus_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bs::pack_<bd::floating_<A0>, X>
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(const saturated_tag &,  const A0& a0, const A0& a1
                                    ) const BOOST_NOEXCEPT
     {
       return a0-a1;
     }
   };
  BOOST_DISPATCH_OVERLOAD_IF(minus_
                         , (typename A0, typename X)
                         , (detail::is_native<X>)
                         , bd::cpu_
                         , bs::saturated_tag
                         , bs::pack_<bd::uint_<A0>, X>
                         , bs::pack_<bd::uint_<A0>, X>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(const saturated_tag &,  const A0& a0, const A0& a1
                                   ) const BOOST_NOEXCEPT
    {
      A0 a0ma1 = a0-a1;
      return if_else_zero(is_less_equal(a0ma1, a0), a0ma1);
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF(minus_
                         , (typename A0, typename X)
                         , (detail::is_native<X>)
                         , bd::cpu_
                         , bs::saturated_tag
                         , bs::pack_<bd::int_<A0>, X>
                         , bs::pack_<bd::int_<A0>, X>
                         )
  {
    BOOST_FORCEINLINE A0 operator()(const saturated_tag &,  const A0& a0, const A0& a1
                                   ) const BOOST_NOEXCEPT
    {
      using utype = bd::as_unsigned_t<A0>;
      using stype = bd::scalar_of_t<A0>;
      utype ux = bitwise_cast<utype>(a0);
      utype uy = bitwise_cast<utype>(a1);
      utype res = ux - uy;
      ux = shr(ux, sizeof(stype)*CHAR_BIT-1) + splat<utype>(Valmax<stype>());
      return bitwise_cast<A0>(if_else(is_ltz(bitwise_cast<A0>(bitwise_and(bitwise_xor(ux,uy)
                                                                         , (bitwise_xor(ux,res))))
                                            )
                                     , ux
                                     , res)
                             );
    }
  };
} } }

#endif


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

using namespace std;

SemiNaiveBayesFeature::SemiNaiveBayesFeature(const HyperParameters &hp) : SemiNaiveBayes( hp ) {
  m_featureType = (hp.naiveBayesFeatureType == "Gaussian") ? FEATURE_GAUSSIAN : FEATURE_HISTOGRAM;
}

void SemiNaiveBayesFeature::calcMeanAndVariance(const matrix<float>& data, const std::vector<int>& labels, SemiFeature& f) {
  cout << "SemiNaiveBayesFeature for Gaussians is not implemented yet!!!" << endl;
  exit(1);
}

// Weighted Statistics
void SemiNaiveBayesFeature::calcMeanAndVariance(const matrix<float>& data, const std::vector<int>& labels, SemiFeature& f,
                                                const std::vector<double>& weights) {
  cout << "SemiNaiveBayesFeature for Gaussians is not implemented yet!!!" << endl;
  exit(1);
}

void SemiNaiveBayesFeature::calcHistogram(const matrix<float>& data, const std::vector<int>& labels, SemiFeature& f) {
  // find min and max values
  int numF = (int) f.indeces.size();
  for (int nF = 0; nF < numF; nF++) {
    f.min.push_back(data(m_inBagSamples[0],f.indeces[nF]));
    f.max.push_back(data(m_inBagSamples[0],f.indeces[nF]));

    BOOST_FOREACH(int sample, m_inBagSamples) {
      f.min[nF] = (data(sample,f.indeces[nF]) < f.min[nF]) ? data(sample,f.indeces[nF]) : f.min[nF];
      f.max[nF] = (data(sample,f.indeces[nF]) > f.max[nF]) ? data(sample,f.indeces[nF]) : f.max[nF];
    }
  }
  f.numBins = (float) m_hp.numHistogramBins;

  std::vector<float> classHistogram((int) pow(f.numBins, numF), 0.0);
  std::vector<std::vector<float> > histogram(m_hp.numClasses, classHistogram);
  std::vector<int> classCounter(m_hp.numClasses,0);
  if (f.numBins > 2.0) {
    for (int nF = 0; nF < numF; nF++) {
      f.w.push_back(abs(f.min[nF] - f.max[nF])/f.numBins);
    }

    BOOST_FOREACH(int sample, m_inBagSamples) {
      int binIndex = 0;
      for (int nF = 0; nF < numF; nF++) {
        binIndex += (int) ((data(sample, f.indeces[nF]) - f.min[nF])/f.w[nF]) + nF*numF;
      }
      binIndex -= (binIndex == pow(f.numBins, numF)) ? 1 : 0;
      histogram[labels[sample]][binIndex]++;
      classCounter[labels[sample]]++;
    }
  } else {
    if (numF > 2) {
      cout << "Exhaustive search for the bins is not possible for more than 2 features!!!" << endl;
      exit(1);
    }
    const float numSteps = 20;
    std::vector<float> stepSize(numF);
    for (int nF = 0; nF < numF; nF++) {
      stepSize[nF] = abs(f.min[nF] - f.max[nF])/numSteps;
    }
    float bestEntropy = 1e10;
    std::vector<float> bestThreshold(numF, 0.0);
    int decision = 0;
    // Find the best threshold
    for (double threshold0 = f.min[0] ; threshold0 < f.max[0] ; threshold0 += stepSize[0]) {
      for (double threshold1 = f.min[1] ; threshold1 < f.max[1] ; threshold1 += stepSize[1]) {
        std::vector<int> count(4, 0);
        std::vector<double> tmpCount(m_hp.numClasses, 0.0);
        std::vector<std::vector<double> > classCount(4, tmpCount);

        BOOST_FOREACH(int sample, m_inBagSamples) {
          if (data(sample, f.indeces[0]) < threshold0 && data(sample, f.indeces[1]) < threshold1) { //00
            decision = 0;
          } else if (data(sample, f.indeces[0]) < threshold0 && data(sample, f.indeces[1]) > threshold1) { //01
            decision = 1;
          } else if (data(sample, f.indeces[0]) > threshold0 && data(sample, f.indeces[1]) < threshold1) { //10
            decision = 2;
          } else if (data(sample, f.indeces[0]) > threshold0 && data(sample, f.indeces[1]) > threshold1) { //11
            decision = 3;
          }

          classCount[decision][labels[sample]]++;
          count[decision]++;
        }

        // Calc entropy
        int totalSampleNum = 0;
        double totalEntropy = 0;
        std::vector<double> entropy(4, 0.0);
        for (int nB = 0; nB < 4; nB++) {
          for (int nClass = 0; nClass < m_hp.numClasses; nClass++) {
            classCount[nB][nClass] /= count[nB];
            classCount[nB][nClass] /= count[nB];

            if (classCount[nB][nClass]) {
              entropy[nB] -= classCount[nB][nClass]*log(classCount[nB][nClass]);
            }
          }

          totalEntropy += count[nB]*entropy[nB];
          totalSampleNum += count[nB];
        }

        // Total Entropy
        totalEntropy /= totalSampleNum;
        if (totalEntropy < bestEntropy) {
          bestEntropy = totalEntropy;
          bestThreshold[0] = threshold0;
          bestThreshold[1] = threshold1;
        }
      }
    }
    f.threshold = bestThreshold;

    BOOST_FOREACH(int sample, m_inBagSamples) {
      int tmpIndex = 0;
      if (data(sample, f.indeces[0]) < f.threshold[0] && data(sample, f.indeces[1]) < f.threshold[1]) { //00
        tmpIndex = 0;
      } else if (data(sample, f.indeces[0]) < f.threshold[0] && data(sample, f.indeces[1]) > f.threshold[1]) { //01
        tmpIndex = 1;
      } else if (data(sample, f.indeces[0]) > f.threshold[0] && data(sample, f.indeces[1]) < f.threshold[1]) { //10
        tmpIndex = 2;
      } else if (data(sample, f.indeces[0]) > f.threshold[0] && data(sample, f.indeces[1]) > f.threshold[1]) { //11
        tmpIndex = 3;
      }

      histogram[labels[sample]][tmpIndex]++;
      classCounter[labels[sample]]++;
    }
  }

  std::vector<std::vector<float> >::iterator it(histogram.begin());
  std::vector<std::vector<float> >::iterator end(histogram.end());
  for (int c = 0;it != end; it++, c++) {
    for (int bin = 0; bin < (int) pow(f.numBins, numF); bin++) {
      (*it)[bin] /= (classCounter[c] + 1e-10);
    }
  }
  f.histogram = histogram;
}

// Weighted Discrete Statistics
void SemiNaiveBayesFeature::calcHistogram(const matrix<float>& data, const std::vector<int>& labels, SemiFeature& f,
                                          const std::vector<double>& weights) {

}

void SemiNaiveBayesFeature::train(const matrix<float>& data, const std::vector<int>& labels,
                                  matrix<float>& forestConfidences, matrix<float>& forestOutOfBagConfidences,
                                  std::vector<int>& forestOutOfBagVoteNum) {
  // Initialize
  initialize(m_hp.numLabeled);

  // Random Subsamples data according to bagratio
  subSample(m_hp.numLabeled);

  // Train the Naive Bayes Classifiers
  SemiFeature f;
  f.type = m_featureType;
  if (m_featureType == FEATURE_GAUSSIAN) {
    cout << "SemiNaiveBayesFeature for Gaussians is not implemented yet!!!" << endl;
    exit(1);
  } else { // FEATURE_HISTOGRAM
    f.indeces = randPerm(data.size2(), m_hp.numRandomFeatures);
    calcHistogram(data,labels,f);
    m_features.push_back(f);
  }

  eval(data,labels);

  finalize(data, forestConfidences, forestOutOfBagConfidences, forestOutOfBagVoteNum);
}

// Weighted Training
void SemiNaiveBayesFeature::train(const matrix<float>& data, const std::vector<int>& labels, const std::vector<double>& weights,
                                  matrix<float>& forestConfidences, matrix<float>& forestOutOfBagConfidences,
                                  std::vector<int>& forestOutOfBagVoteNum) {
  // Initialize
  initialize(m_hp.numLabeled);

  // Random Subsamples data according to bagratio
  subSample(m_hp.numLabeled);

  // Train the Naive Bayes Classifiers
  SemiFeature f;
  f.type = m_featureType;
  if (m_featureType == FEATURE_GAUSSIAN) {
    cout << "SemiNaiveBayesFeature for Gaussians is not implemented yet!!!" << endl;
    exit(1);
  } else { // FEATURE_HISTOGRAM
    f.indeces = randPerm(data.size2(),m_hp.numRandomFeatures);
    calcHistogram(data,labels,f,weights);
    m_features.push_back(f);
  }

  eval(data,labels);

  finalize(data, forestConfidences, forestOutOfBagConfidences, forestOutOfBagVoteNum);
}

void SemiNaiveBayesFeature::evalOutOfBagSamples(const matrix<float>& data) {

}

void SemiNaiveBayesFeature::eval(const matrix<float>& data, const std::vector<int> labels) {
  m_confidences.resize(data.size1(), m_hp.numClasses);
  // init to one (due to multiplication)
  for (int sample = 0; sample < (int)data.size1(); sample++) {
    for ( int c = 0; c < m_hp.numClasses; c++) {
      m_confidences(sample,c) = 1.0;
    }
  }

  // Fill confidence matrix
  std::vector<SemiFeature>::iterator it(m_features.begin());
  std::vector<SemiFeature>::iterator end(m_features.end());
  for (int sample = 0; sample < (int)data.size1(); sample++) {
    it = m_features.begin();
    while (it != end) {
      it->eval(data,sample,m_confidences);
      ++it;
    }
    double max = 0.0;
    for ( int c = 0; c < m_hp.numClasses; c++) {
      if (m_confidences(sample,c) > max) {
        m_predictions[sample] = c;
        max = m_confidences(sample,c);
      }
    }
  }

  double error = computeError(labels);
  if (m_hp.verbose) {
    cout << "Error: " << error << endl;
  }

}

void SemiNaiveBayesFeature::eval(const matrix<float>& data, matrix<float>& confidences) {
  m_confidences.resize(data.size1(), m_hp.numClasses);
  // init to one (due to multiplication)
  for (int sample = 0; sample < (int)data.size1(); sample++) {
    for ( int c = 0; c < m_hp.numClasses; c++) {
      m_confidences(sample,c) = 1.0;
    }
  }

  // Fill confidence matrix
  std::vector<SemiFeature>::iterator it(m_features.begin());
  std::vector<SemiFeature>::iterator end(m_features.end());
  for (int sample = 0; sample < (int)data.size1(); sample++) {
    it = m_features.begin();
    while (it != end) {
      it->eval(data,sample,m_confidences);
      ++it;
    }
    for ( int c = 0; c < m_hp.numClasses; c++) {
      confidences(sample,c) += m_confidences(sample,c);
    }
  }

}

void SemiFeature::eval(const matrix<float>& data, const int sampleIndex, matrix<float>& confidences) {
  if ( type == FEATURE_GAUSSIAN ) {
    cout << "SemiNaiveBayesFeature for Gaussians is not implemented yet!!!" << endl;
    exit(1);
  } else {
    evalHistogram(data,sampleIndex,confidences);
  }
}

void SemiFeature::evalGaussian(const matrix<float>& data, const int sampleIndex, matrix<float>& confidences) {
  cout << "SemiNaiveBayesFeature for Gaussians is not implemented yet!!!" << endl;
  exit(1);
}

void SemiFeature::evalHistogram(const matrix<float>& data, const int sampleIndex, matrix<float>& confidences) {
  int tmpIndex;
  if (numBins > 2) {
    tmpIndex = 0;
    for (int nF = 0; nF < (int) indeces.size(); nF++) {
      tmpIndex += (int) ((data(sampleIndex, indeces[nF]) - min[nF])/w[nF]) + nF*indeces.size();
    }
  }
  else {
    if (data(sampleIndex, indeces[0]) < threshold[0] && data(sampleIndex, indeces[1]) < threshold[1]) { //00
      tmpIndex = 0;
    } else if (data(sampleIndex, indeces[0]) < threshold[0] && data(sampleIndex, indeces[1]) > threshold[1]) { //01
      tmpIndex = 1;
    } else if (data(sampleIndex, indeces[0]) > threshold[0] && data(sampleIndex, indeces[1]) < threshold[1]) { //10
      tmpIndex = 2;
    } else if (data(sampleIndex, indeces[0]) > threshold[0] && data(sampleIndex, indeces[1]) > threshold[1]) { //11
      tmpIndex = 3;
    }
  }

  for (int c = 0; c < (int)histogram.size(); c++) {
    confidences(sampleIndex,c) *= histogram[c][tmpIndex];
  }
}