#include <iostream>
#include <sstream>
#include <cfloat>
#include <set>

#include <armadillo>

#include <cpp-argparse/OptionParser.h>
#include <besiq/io/covariates.hpp>
#include <besiq/stats/snp_count.hpp>
#include <glm/irls.hpp>
#include <glm/models/normal.hpp>
#include <glm/models/binomial.hpp>

#include <plink/plink_file.hpp>
#include <dcdflib/libdcdf.hpp>

using namespace arma;
using namespace optparse;

const std::string USAGE = "besiq-mglm [OPTIONS] plink_file";
const std::string DESCRIPTION = "Multivariate additive GLM.";
const std::string VERSION = "besiq 0.0.1";
const std::string EPILOG = "";

std::set<std::string>
parse_variants(std::ifstream &input_file)
{
    std::set<std::string> variant_set;
    std::string variant;
    while( input_file >> variant )
    {
        variant_set.insert( variant );
    }

    return variant_set;
}

std::vector<std::string>
create_design_matrix(genotype_matrix_ptr genotypes, const arma::mat &cov, const std::set<std::string> &variant_set, arma::uvec &missing, arma::mat &X)
{

    bool all_variants = variant_set.size( ) == 0;
    size_t num_variants = variant_set.size( );
    if( all_variants )
    {
        num_variants = genotypes->size( );
    }

    X.resize( missing.n_elem, num_variants + cov.n_cols + 1 );

    std::vector<std::string> names = genotypes->get_snp_names( );
    std::vector<std::string> valid_names;
    size_t cur_col = 0;
    for(int i = 0; i < genotypes->size( ); i++)
    {
        if( !all_variants && variant_set.count( names[ i ] ) <= 0 )
        {
            continue;
        }

        snp_row &cur_row = genotypes->get_row( i );
        for(int j = 0; j < cur_row.size( ); j++)
        {
            if( cur_row[ j ] != 3 )
            {
                X( j, cur_col ) = cur_row[ j ];
            }
            else
            {
                X( j, cur_col ) = 0;
                missing[ j ] = 1;
            }
        }

        if( arma::var( X.col( cur_col ) ) > 1e-4 )
        {
            valid_names.push_back( names[ i ] );
            cur_col++;
        }
    }

    for(int i = 0; i < cov.n_cols; i++)
    {
        X.col( cur_col ) = cov.col( i );
        cur_col++;
    }

    X.col( cur_col ) = arma::ones<arma::vec>( missing.n_elem );
    cur_col++;
    X.resize( missing.n_elem, cur_col );

    return valid_names;
}

int
main(int argc, char *argv[])
{
    char const* const model_choices[] = { "binomial", "normal" };
    char const* const link_choices[] = { "logit", "logc", "odds", "identity", "log" };
    OptionParser parser = OptionParser( ).usage( USAGE )
                                         .version( VERSION )
                                         .description( DESCRIPTION )
                                         .epilog( EPILOG );

    parser.add_option( "-p", "--pheno" ).help( "Read phenotypes from this file instead of a plink file." );
    parser.add_option( "-e", "--mpheno" ).help( "Name of the phenotype that you want to read (if there are more than one in the phenotype file)." );
    parser.add_option( "-c", "--cov" ).action( "store" ).type( "string" ).metavar( "filename" ).help( "Performs the analysis by including the covariates in this file." );
    parser.add_option( "-s", "--variants" ).action( "store" ).type( "string" ).metavar( "filename" ).help( "Only use these variants." );
    parser.add_option( "-o", "--out" ).help( "The output file that will contain the results (binary)." );
    parser.add_option( "-t", "--threshold" ).help( "Only output p-values less or equal to this value." ).set_default( 1.0 );
    parser.add_option( "-m", "--model" ).choices( &model_choices[ 0 ], &model_choices[ 2 ] ).metavar( "model" ).help( "The model to use for the phenotype, 'binomial' or 'normal', default = 'binomial'." ).set_default( "binomial" );
    parser.add_option( "-l", "--link-function" ).choices( &link_choices[ 0 ], &link_choices[ 5 ] ).metavar( "link" ).help( "The link function, or scale, that is used for the penetrance: 'logit' log(p/(1-p)), 'logc' log(1 - p), 'odds' p/(1-p), 'identity' p, 'log' log(p)." );

    Values options = parser.parse_args( argc, argv );
    if( parser.args( ).size( ) != 1 )
    {
        parser.print_help( );
        exit( 1 );
    }

    std::ios_base::sync_with_stdio( false );

    /* Read all genotypes */
    plink_file_ptr genotype_file = open_plink_file( parser.args( )[ 0 ] );
    genotype_matrix_ptr genotypes = create_genotype_matrix( genotype_file );
    std::vector<std::string> order = genotype_file->get_sample_iids( );

    /* Make error streams separate from stdout */
    arma::set_stream_err1( std::cerr );
    arma::set_stream_err2( std::cerr );

    /* Parse phenotypes */
    arma::uvec missing = arma::zeros<arma::uvec>( genotype_file->get_samples( ).size( ) );
    arma::vec phenotype;
    arma::mat cov;
    std::vector<std::string> cov_names;
    if( options.is_set( "pheno" ) )
    {
        std::ifstream phenotype_file( options[ "pheno" ].c_str( ) );
        phenotype = parse_phenotypes( phenotype_file, missing, order, options[ "mpheno" ] );
    }
    else
    {
        phenotype = create_phenotype_vector( genotype_file->get_samples( ), missing );
    }
    if( options.is_set( "cov" ) )
    {
        std::ifstream covariate_file( options[ "cov" ].c_str( ) );
        cov = parse_covariate_matrix( covariate_file, missing, order, &cov_names );
    }

    std::set<std::string> variant_set;
    if( options.is_set( "variants" ) )
    {
        std::ifstream variant_file( options[ "variants" ].c_str( ) );
        variant_set = parse_variants( variant_file );
    }

    /* Open output stream */
    std::ofstream output_file;
    if( options.is_set( "out" ) )
    {
        output_file.open( options[ "out" ].c_str( ) );
    }
    std::ostream &out = options.is_set( "out" ) ? output_file : std::cout;

    arma::mat X;
    std::vector<std::string> variant_names = create_design_matrix( genotypes, cov, variant_set, missing, X );

    glm_model *model = NULL;
    if( options[ "model" ] == "binomial" )
    {
        std::string link = "logit";
        if( options.is_set( "link_function" ) )
        {
            link = options[ "link_function" ];
        }

        model= new binomial( link );
    }
    else if( options[ "model" ] == "normal" )
    {
        std::string link = "identity";
        if( options.is_set( "link_function" ) )
        {
            link = options[ "link_function" ];
        }

        model = new normal( link );
    }

    glm_info result;
    arma::vec beta = irls( X, phenotype, missing, *model, result);

    /* The design matrix is constructed such that the variants appear first */
    out << "snp\tbeta\tse_beta\tpvalue\n";
    if( result.success && result.converged )
    {
        for(int i = 0; i < variant_names.size( ); i++)
        {
            out << variant_names[ i ] << "\t" <<
                beta[ i ] << "\t" <<
                result.se_beta[ i ] << "\t" <<
                result.p_value[ i ] << "\n";
        }
    }

    if( model != NULL )
    {
        delete model;
    }

    return 0;
}


#include "Sensor.h"
#include <boost/iterator/iterator_concepts.hpp>

using namespace A2O;
using namespace Eigen;

Sensor::Sensor(ISensorConfig::ConstPtr config)
      : Sensor(config->getName(), config->getPerceptorName(), config->getPose())
{

}

Sensor::Sensor(const std::string& name,
	       const std::string& perceptorName,
	       const Pose3D& pose)
      : _name(name), _perceptorName(perceptorName), _lastMeasurementTime(0), _pose(pose)
{

}

Sensor::~Sensor()
{

}

const std::string& Sensor::getName() const
{
  return _name;
}

const std::string& Sensor::getPerceptorName() const
{
  return _perceptorName;
}

const Pose3D& Sensor::getPose() const
{
  return _pose;
}

const long& Sensor::getLastMeasurementTime() const
{
  return _lastMeasurementTime;
}

const Vector3d Sensor::toCarSystem(const Vector3d& localPos) const
{
  return _pose * localPos;
}

const Quaterniond Sensor::toCarSystem(const Quaterniond& localOrientation) const
{
  return _pose.getOrientation() * localOrientation;
}

const Pose3D Sensor::toCarSystem(const Pose3D& localPose) const
{
  return _pose * localPose;
}

const bool Sensor::isInitialized() const
{
  return true;
}


//
// Created by june on 19. 12. 5..
//

// from ros-control meta packages
#include <ros/ros.h>
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <pluginlib/class_list_macros.h>
#include <urdf/model.h>
#include <sensor_msgs/JointState.h>
#include <realtime_tools/realtime_buffer.h>
#include <realtime_tools/realtime_publisher.h>
#include "dualarm_controller/JointDesiredState.h"
#include "dualarm_controller/JointCurrentState.h"
#include "utils.h"

// from kdl packages
#include <kdl/tree.hpp>
#include <kdl/chain.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chaindynparam.hpp>              // inverse dynamics
#include <kdl/chainjnttojacsolver.hpp>        // jacobian
#include <kdl/chainfksolverpos_recursive.hpp> // forward kinematics

#include <boost/scoped_ptr.hpp>
#include <boost/lexical_cast.hpp>

#include <cmath>
#define _USE_MATH_DEFINES

#include <SerialManipulator.h>
#include <Controller.h>
#include <Motion.h>

#define D2R M_PI/180.0
#define R2D 180.0/M_PI

#define A 0.25
#define b1 0.451
#define b2 -0.316
#define b3 0.843
#define f 0.3

#define l_p1 0.470
#define l_p2 0.40
#define l_p3 0.812

#define Deg_A 70
#define Deg_f 0.5

namespace dualarm_controller
{
    class JointSpace_Control : public controller_interface::Controller<hardware_interface::EffortJointInterface>
    {
    public:
        bool init(hardware_interface::EffortJointInterface *hw, ros::NodeHandle &n)
        {
            // ********* 1. Get joint name / gain from the parameter server *********

            // 1.1 Joint Name
            if (!n.getParam("joints", joint_names_))
            {
                ROS_ERROR("Could not find joint name");
                return false;
            }
            n_joints_ = joint_names_.size();

            if (n_joints_ == 0)
            {
                ROS_ERROR("List of joint names is empty.");
                return false;
            }
            else
            {
                ROS_INFO("Found %d joint names", n_joints_);
                for (int i = 0; i < n_joints_; i++)
                {
                    ROS_INFO("%s", joint_names_[i].c_str());
                }
            }

            // 1.2 Gain
            // 1.2.1 Joint Controller
            Kp_.resize(n_joints_);
            Kd_.resize(n_joints_);
            Ki_.resize(n_joints_);
            K_inf_.resize(n_joints_);
            aKp_.resize(n_joints_);
            aKd_.resize(n_joints_);
            aKi_.resize(n_joints_);
            aK_inf_.resize(n_joints_);
            std::vector<double> Kp(n_joints_), Ki(n_joints_), Kd(n_joints_), K_inf(n_joints_);

            for (size_t i = 0; i < n_joints_; i++)
            {
                std::string si = std::to_string(i + 1);
                if (n.getParam("/dualarm/jointspace_control/gains/dualarm_joint" + si + "/pid/p", Kp[i]))
                {
                    Kp_(i) = Kp[i];
                }
                else
                {
                    std::cout << "/dualarm/jointspace_control/gains/dualarm_joint" + si + "/pid/p" << std::endl;
                    ROS_ERROR("Cannot find pid/p gain");
                    return false;
                }

                if (n.getParam("/dualarm/jointspace_control/gains/dualarm_joint" + si + "/pid/i", Ki[i]))
                {
                    Ki_(i) = Ki[i];
                }
                else
                {
                    ROS_ERROR("Cannot find pid/i gain");
                    return false;
                }

                if (n.getParam("/dualarm/jointspace_control/gains/dualarm_joint" + si + "/pid/d", Kd[i]))
                {
                    Kd_(i) = Kd[i];
                }
                else
                {
                    ROS_ERROR("Cannot find pid/d gain");
                    return false;
                }

                if (n.getParam("/dualarm/jointspace_control/gains/dualarm_joint" + si + "/pid/h", K_inf[i]))
                {
                    K_inf_(i) = K_inf[i];
                }
                else
                {
                    ROS_ERROR("Cannot find pid/h gain");
                    return false;
                }
            }

            // 2. ********* urdf *********
            urdf::Model urdf;
            if (!urdf.initParam("robot_description"))
            {
                ROS_ERROR("Failed to parse urdf file");
                return false;
            }
            else
            {
                ROS_INFO("Found robot_description");
            }

            // 3. ********* Get the joint object to use in the realtime loop [Joint Handle, URDF] *********
            for (int i = 0; i < n_joints_; i++)
            {
                try
                {
                    joints_.push_back(hw->getHandle(joint_names_[i]));
                }
                catch (const hardware_interface::HardwareInterfaceException &e)
                {
                    ROS_ERROR_STREAM("Exception thrown: " << e.what());
                    return false;
                }

                urdf::JointConstSharedPtr joint_urdf = urdf.getJoint(joint_names_[i]);
                if (!joint_urdf)
                {
                    ROS_ERROR("Could not find joint '%s' in urdf", joint_names_[i].c_str());
                    return false;
                }
                joint_urdfs_.push_back(joint_urdf);
            }

            // 4. ********* KDL *********
            // 4.1 kdl parser
            if (!kdl_parser::treeFromUrdfModel(urdf, kdl_tree_))
            {
                ROS_ERROR("Failed to construct kdl tree");
                return false;
            }
            else
            {
                ROS_INFO("Constructed kdl tree");
            }

            // 4.2 kdl chain
            std::string root_name, tip_name1, tip_name2;
            if (!n.getParam("root_link", root_name))
            {
                ROS_ERROR("Could not find root link name");
                return false;
            }
            if (!n.getParam("tip_link1", tip_name1))
            {
                ROS_ERROR("Could not find tip link name");
                return false;
            }
            if (!n.getParam("tip_link2", tip_name2))
            {
                ROS_ERROR("Could not find tip link name");
                return false;
            }

            if (!kdl_tree_.getChain(root_name, tip_name1, kdl_chain_))
            {
                ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
                ROS_ERROR_STREAM("  " << root_name << " --> " << tip_name1);
                ROS_ERROR_STREAM("  Chain has " << kdl_chain_.getNrOfJoints() << " joints");
                ROS_ERROR_STREAM("  Chain has " << kdl_chain_.getNrOfSegments() << " segments");
                ROS_ERROR_STREAM("  The segments are:");

                KDL::SegmentMap segment_map = kdl_tree_.getSegments();
                KDL::SegmentMap::iterator it;

                for (it = segment_map.begin(); it != segment_map.end(); it++)
                    ROS_ERROR_STREAM("    " << (*it).first);

                return false;
            }
            else
            {
                ROS_INFO_STREAM("Got kdl first chain");
                ROS_INFO_STREAM("  " << root_name << " --> " << tip_name1);
                ROS_INFO_STREAM("  Chain has " << kdl_chain_.getNrOfJoints() << " joints");
                ROS_INFO_STREAM("  Chain has " << kdl_chain_.getNrOfSegments() << " segments");
            }

            if(!kdl_tree_.getChain(root_name, tip_name2, kdl_chain2_))
            {
                ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
                ROS_ERROR_STREAM("  " << root_name << " --> " << tip_name2);
                ROS_ERROR_STREAM("  Chain has " << kdl_chain2_.getNrOfJoints() << " joints");
                ROS_ERROR_STREAM("  Chain has " << kdl_chain2_.getNrOfSegments() << " segments");
                ROS_ERROR_STREAM("  The segments are:");

                KDL::SegmentMap segment_map = kdl_tree_.getSegments();
                KDL::SegmentMap::iterator it;

                for (it = segment_map.begin(); it != segment_map.end(); it++)
                    ROS_ERROR_STREAM("    " << (*it).first);

                return false;
            }
            else
            {
                ROS_INFO_STREAM("Got kdl second chain");
                ROS_INFO_STREAM("  " << root_name << " --> " << tip_name2);
                ROS_INFO_STREAM("  Chain has " << kdl_chain2_.getNrOfJoints() << " joints");
                ROS_INFO_STREAM("  Chain has " << kdl_chain2_.getNrOfSegments() << " segments");
            }

            // 4.3 inverse dynamics solver 초기화
            g_kdl_ = KDL::Vector::Zero();
            g_kdl_(2) = -9.81; // 0: x-axis 1: y-axis 2: z-axis

            id_solver_.reset(new KDL::ChainDynParam(kdl_chain_, g_kdl_));
            jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
            fk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
            J1_kdl_.resize(kdl_chain_.getNrOfJoints());
            M_kdl_.resize(kdl_chain_.getNrOfJoints());
            C_kdl_.resize(kdl_chain_.getNrOfJoints());
            G_kdl_.resize(kdl_chain_.getNrOfJoints());

            id_solver1_.reset(new KDL::ChainDynParam(kdl_chain2_, g_kdl_));
            jnt_to_jac_solver1_.reset(new KDL::ChainJntToJacSolver(kdl_chain2_));
            fk_pos_solver1_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain2_));
            J2_kdl_.resize(kdl_chain2_.getNrOfJoints());
            M1_kdl_.resize(kdl_chain2_.getNrOfJoints());
            C1_kdl_.resize(kdl_chain2_.getNrOfJoints());
            G1_kdl_.resize(kdl_chain2_.getNrOfJoints());

            // ********* 5. 각종 변수 초기화 *********
            // 5.1 KDL Vector 초기화 (사이즈 정의 및 값 0)
            qd_.data = Eigen::VectorXd::Zero(n_joints_);
            qd_dot_.data = Eigen::VectorXd::Zero(n_joints_);
            qd_ddot_.data = Eigen::VectorXd::Zero(n_joints_);

            q_.data = Eigen::VectorXd::Zero(n_joints_);
            qdot_.data = Eigen::VectorXd::Zero(n_joints_);
            dq.setZero(n_joints_);
            frictiontorque_.data = Eigen::VectorXd::Zero(n_joints_);

            des_torque.setZero(n_joints_);
            act_torque.setZero(n_joints_);
//            frictiontorque.setZero(n_joints_);

            targetpos.setZero(16);

            // ********* 6. ROS 명령어 *********
            // 6.1 publisher
            state_pub_.reset(new realtime_tools::RealtimePublisher<dualarm_controller::JointCurrentState>(n, "states", 10));
            state_pub_->msg_.header.stamp = ros::Time::now();
            for(int i=0; i<(n_joints_); i++)
            {
                state_pub_->msg_.name.push_back(joint_names_[i].c_str()) ;
                state_pub_->msg_.q.push_back(q_.data(i));
                state_pub_->msg_.qdot.push_back(qdot_.data(i));
                state_pub_->msg_.dq.push_back(qd_.data(i));
                state_pub_->msg_.dqdot.push_back(qd_dot_.data(i));
                state_pub_->msg_.dqddot.push_back(qd_ddot_.data(i));

                state_pub_->msg_.effort_command.push_back(act_torque(i));
                state_pub_->msg_.Kp.push_back(Kp_.data(i));
                state_pub_->msg_.Kd.push_back(Kd_.data(i));
                state_pub_->msg_.frictiontorque.push_back(frictiontorque_.data(i));

            }
            pub_buffer_.writeFromNonRT(std::vector<double>(n_joints_, 0.0));

            // 6.2 subsriber
            const auto joint_state_cb = utils::makeCallback<dualarm_controller::JointDesiredState>([&](const auto& msg){
                for(int i=0; i<n_joints_; i++)
                {
                    ControlSubIndex = msg.SubIndex;
                    JointState = ControlSubIndex;
                    targetpos(i)=msg.dq[i].data*D2R;
                    dq(i) = msg.dq[i].data*D2R;
//                    Kp_.data(i) = msg.Kp[i].data;
//                    Kd_.data(i) = msg.Kd[i].data;
                }
                qd_.data = dq;
            });
            sub_x_cmd_ = n.subscribe<dualarm_controller::JointDesiredState>( "command", 5, joint_state_cb);

            return true;
        }

        void starting(const ros::Time &time) override
        {
            t = 0.0;
            InitTime=5.0;

            ROS_INFO_STREAM("Starting Joint-Space Controller");

            cManipulator = std::make_shared<SerialManipulator>();
            Control = std::make_unique<HYUControl::Controller>(cManipulator);
            motion = std::make_unique<HYUControl::Motion>(cManipulator);
            ControlSubIndex = MOVE_ZERO;
            JointState = MOVE_ZERO;
            cManipulator->UpdateManipulatorParam();
        }

        void update(const ros::Time &time, const ros::Duration &period) override
        {
            std::vector<double> &commands = *pub_buffer_.readFromRT();
            // ********* 0. Get states from gazebo *********
            // 0.1 sampling time
            double dt = period.toSec();

            // 0.2 joint state
            for (int i = 0; i < n_joints_; i++)
            {
                q_(i) = joints_[i].getPosition();
                qdot_(i) = joints_[i].getVelocity();
                act_torque(i) = joints_[i].getEffort();
            }

            //----------------------
            // dynamics calculation
            //----------------------
            cManipulator->pKin->PrepareJacobian(q_.data);
            cManipulator->pDyn->PrepareDynamics(q_.data, qdot_.data);
            cManipulator->pDyn->MG_Mat_Joint(M, G);
            //cManipulator->pDyn->C_Matrix(C);

            cManipulator->pKin->GetForwardKinematics(ForwardPos, ForwardOri, NumChain);
            cManipulator->pKin->GetAngleAxis(ForwardAxis, ForwardAngle, NumChain);
            Control->SetPIDGain(Kp_.data, Kd_.data, Ki_.data, K_inf_.data);
            Control->GetPIDGain(aKp_.data, aKd_.data, aKi_.data);

            q1_.data = q_.data.head(9);
            q1dot_.data = qdot_.data.head(9);
            jnt_to_jac_solver_->JntToJac(q1_, J1_kdl_);
            id_solver_->JntToMass(q1_, M_kdl_);
            id_solver_->JntToCoriolis(q1_, q1dot_, C_kdl_);
            id_solver_->JntToGravity(q1_, G_kdl_);
            fk_pos_solver_->JntToCart(q1_, x_[0]);

            q2_.resize(9);
            q2dot_.resize(9);
            q2_.data.tail(7) = q_.data.tail(7);
            q2_.data.head(2) = q_.data.head(2);
            q2dot_.data = qdot_.data.tail(7);
            q2dot_.data.head(2) = qdot_.data.head(2);
            jnt_to_jac_solver1_->JntToJac(q2_, J2_kdl_);
            id_solver1_->JntToMass(q2_, M1_kdl_);
            id_solver1_->JntToCoriolis(q2_, q2dot_, C1_kdl_);
            id_solver1_->JntToGravity(q2_, G1_kdl_);
            fk_pos_solver1_->JntToCart(q2_, x_[1]);

            if( t <= InitTime )
            {
                qd_.data.setZero();

                qd_.data(0) = -0.0*D2R;
                qd_.data(1) = -0.0*D2R;

                qd_.data(2) = 0.0*D2R;
                qd_.data(3) = -20.0*D2R;
                qd_.data(4) = -0.0*D2R;
                qd_.data(5) = -0.0*D2R;
                qd_.data(6) = -70.0*D2R;
                qd_.data(7) = 0.0*D2R;
                qd_.data(8) = 0.0*D2R;

                qd_.data(9) = 0.0*D2R;
                qd_.data(10) = 20.0*D2R;
                qd_.data(11) = 0.0*D2R;
                qd_.data(12) = -0.0*D2R;
                qd_.data(13) = 70.0*D2R;
                qd_.data(14) = -0.0*D2R;
                qd_.data(15) = -0.0*D2R;
            }

            qd_ddot_.data.setZero();
            qd_dot_.data.setZero();
            motion->JointMotion(qd_.data, qd_dot_.data, qd_ddot_.data, targetpos, q_.data, qdot_.data, t, JointState, ControlSubIndex);
            Control->InvDynController2(q_.data, qdot_.data, qd_.data, qd_dot_.data, qd_ddot_.data, des_torque, frictiontorque_.data, dt);
            e=qd_.data-q_.data;
            for (int i = 0; i < n_joints_; i++)
            {
                if(des_torque(i) >= 100)
                    des_torque(i) = 100;
                else if(des_torque(i) <= -100)
                    des_torque(i) = -100;

                joints_[i].setCommand(des_torque(i));
            }

            // ********* 4. data 저장 *********
            publish_data();

            // ********* 5. state 출력 *********
            print_state();

            t = t + dt;
        }

        void stopping(const ros::Time &time) override
        {
            ROS_INFO_STREAM("Stop Joint-Space Controller");
        }

        void publish_data()
        {
            static int loop_count_ = 0;
            if(loop_count_ > 2)
            {
                if(state_pub_->trylock())
                {
                    state_pub_->msg_.header.stamp = ros::Time::now();
                    for(size_t i=0; i<(n_joints_-1); i++)
                    {
                        state_pub_->msg_.q[i] = q_.data(i);
                        state_pub_->msg_.qdot[i] = qdot_.data(i);
                        state_pub_->msg_.dq[i] = qd_.data(i);
                        state_pub_->msg_.dqdot[i] = qd_dot_.data(i);
                        state_pub_->msg_.dqddot[i] = qd_ddot_.data(i);

                        state_pub_->msg_.effort_command[i] = act_torque(i);
                        state_pub_->msg_.frictiontorque[i] = frictiontorque_.data(i);

                        state_pub_->msg_.Kp[i] = Kp_.data(i);
                        state_pub_->msg_.Kd[i] = Kd_.data(i);
                    }
                    state_pub_->unlockAndPublish();
                }
                loop_count_=0;
            }
            loop_count_++;
        }

        void print_state()
        {
            static int count = 0;
            if (count > 499)
            {
                printf("*********************************************************\n\n");
                printf("*** Simulation Time (unit: sec)  ***\n");
                printf("t = %f\n", t);
                printf("\n");

                printf("*** States in Joint Space (unit: deg) ***\n");

                for(int i=0; i < n_joints_; i++)
                {
                    printf("[%s]:  \t", joint_names_[i].c_str());
                    printf("q: %0.3lf,\t", q_.data(i) * R2D);
                    printf("dq: %0.3lf,\t", qd_.data(i) * R2D);
                    printf("e: %0.3lf,\t", e(i) * R2D);

                    printf("qdot: %0.3lf,\t", qdot_.data(i) * R2D);
//                    printf("dqdot: %0.3lf,\t", qd_dot_.data(i) * R2D);
                    printf("tau: %0.3f\t", des_torque(i));
                    printf("tau_friction: %0.3f\t", frictiontorque_.data(i));

//                    printf("Kp:%0.1lf, Kd:%0.1lf, Ki:%0.1lf\t", aKp_.data(i), aKd_.data(i),  aKi_.data(i));
                    printf("SubIndex:%d\n", ControlSubIndex);

                }

                printf("\nForward Kinematics:\n");
                for(int j=0; j<NumChain; j++)
                {
                    printf("no.%d, PoE: x:%0.3lf, y:%0.3lf, z:%0.3lf, u:%0.2lf, v:%0.2lf, w:%0.2lf\n", j,
                           ForwardPos[j](0), ForwardPos[j](1),ForwardPos[j](2),
                           ForwardOri[j](0), ForwardOri[j](1), ForwardOri[j](2));
                    double a, b, g;
                    x_[j].M.GetEulerZYX(a, b, g);
                    printf("no.%d, DH: x:%0.3lf, y:%0.3lf, z:%0.3lf, u:%0.2lf, v:%0.2lf, w:%0.2lf\n",
                           j, x_[j].p(0), x_[j].p(1),x_[j].p(2), g, b, a);
                    printf("no.%d, AngleAxis x: %0.2lf, y: %0.2lf, z: %0.2lf, Angle: %0.3lf\n\n",
                           j, ForwardAxis[j](0), ForwardAxis[j](1), ForwardAxis[j](2), ForwardAngle[j]);
                }
                printf("\n*********************************************************\n");
                count = 0;
                /*
                M_mat_collect.resize(16,16);
                M_mat_collect.setZero();
                M_mat_collect.block(0,0,2,2) = M_kdl_.data.block(0,0,2,2) + M1_kdl_.data.block(0,0,2,2);
                M_mat_collect.block(0,2,2,7) = M_kdl_.data.block(0,2,2,7);
                M_mat_collect.block(0,9,2,7) = M1_kdl_.data.block(0,2,2,7);
                M_mat_collect.block(2,0,7,2) = M_kdl_.data.block(2,0,7,2);
                M_mat_collect.block(9,0,7,2) = M1_kdl_.data.block(2,0,7,2);
                M_mat_collect.block(2,2,7,7) = M_kdl_.data.block(2,2,7,2);
                M_mat_collect.block(9,9,7,7) = M1_kdl_.data.block(2,2,7,2);
                std::cout << "M_kdl" << std::endl;
                std::cout << M_mat_collect << std::endl;
                std::cout << "M_PoE" << std::endl;
                std::cout << M << std::endl;
                */
                /*
                C_mat_collect.resize(16,16);
                C_mat_collect.setZero();
                C_mat_collect.block(0,0,2,2) = C_kdl_.data.block(0,0,2,2) + C1_kdl_.data.block(0,0,2,2);
                C_mat_collect.block(0,2,2,7) = C_kdl_.data.block(0,2,2,7);
                C_mat_collect.block(0,9,2,7) = C1_kdl_.data.block(0,2,2,7);
                C_mat_collect.block(2,0,7,2) = C_kdl_.data.block(2,0,7,2);
                C_mat_collect.block(9,0,7,2) = C1_kdl_.data.block(2,0,7,2);
                C_mat_collect.block(2,2,7,7) = C_kdl_.data.block(2,2,7,2);
                C_mat_collect.block(9,9,7,7) = C1_kdl_.data.block(2,2,7,2);
                std::cout << "C_kdl" << std::endl;
                std::cout << C_mat_collect << std::endl;
                std::cout << "C_PoE" << std::endl;
                std::cout << C << std::endl;
                */

            }
            count++;
        }

    private:
        // others
        double t;
        double InitTime=0;

        //Joint handles
        unsigned int n_joints_;
        unsigned char ControlSubIndex;
        unsigned char JointState;

        std::vector<std::string> joint_names_;
        std::vector<hardware_interface::JointHandle> joints_;
        std::vector<urdf::JointConstSharedPtr> joint_urdfs_;

        // kdl
        KDL::Tree kdl_tree_;
        KDL::Chain kdl_chain_;
        KDL::Chain kdl_chain2_;

        KDL::JntSpaceInertiaMatrix M_kdl_, M1_kdl_;
        KDL::JntArray C_kdl_, C1_kdl_;
        KDL::JntArray G_kdl_, G1_kdl_;
        KDL::Vector g_kdl_;
        Eigen::VectorXd g_vec_collect;
        Eigen::MatrixXd g_mat_collect;
        Eigen::MatrixXd M_mat_collect;
        Eigen::MatrixXd C_mat_collect;
        Eigen::VectorXd e, e_dev, e_int, e_int_sat;

        KDL::Jacobian J1_kdl_, J2_kdl_;

        // kdl solver
        boost::scoped_ptr<KDL::ChainFkSolverPos_recursive> fk_pos_solver_, fk_pos_solver1_;     // Solver to compute the forward kinematics (position)
        boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_, jnt_to_jac_solver1_;    // Solver to compute the jacobian
        boost::scoped_ptr<KDL::ChainDynParam> id_solver_, id_solver1_;                          // Solver To compute the inverse dynamics

        MatrixXd M;
        VectorXd G;
        MatrixXd C;

        Vector3d ForwardPos[2];
        Vector3d ForwardOri[2];
        int NumChain=2;
        Vector3d ForwardAxis[2];
        double ForwardAngle[2];

        // Joint Space State
        KDL::JntArray qd_;
        KDL::JntArray qd_dot_;
        KDL::JntArray qd_ddot_;
        KDL::JntArray q_, q1_, q2_;
        KDL::JntArray qdot_, q1dot_, q2dot_;
        KDL::JntArray frictiontorque_;

        Eigen::VectorXd targetpos;

        VectorXd dq;
        VectorXd des_torque;
        VectorXd act_torque;
        VectorXd frictiontorque;

        // Task Space State
        KDL::Frame x_[2];

        // gains
        KDL::JntArray Kp_, Ki_, Kd_, K_inf_;
        KDL::JntArray aKp_, aKi_, aKd_, aK_inf_;

        // publisher
        realtime_tools::RealtimeBuffer<std::vector<double>> pub_buffer_;
        boost::scoped_ptr<realtime_tools::RealtimePublisher<dualarm_controller::JointCurrentState>> state_pub_;

        // subscriber
        ros::Subscriber sub_x_cmd_;

        std::shared_ptr<SerialManipulator> cManipulator;
        std::unique_ptr<HYUControl::Controller> Control;
        std::unique_ptr<HYUControl::Motion> motion;

    };
}

PLUGINLIB_EXPORT_CLASS(dualarm_controller::JointSpace_Control,controller_interface::ControllerBase)


//
//  ExporterOBJ.cpp
//  Kuplung
//
//  Created by Sergey Petrov on 12/16/15.
//  Copyright © 2015 supudo.net. All rights reserved.
//

#include "ExporterOBJ.hpp"
#include <boost/algorithm/string.hpp>
#include <boost/algorithm/string/predicate.hpp>
#include <fstream>
#include <glm/gtx/matrix_decompose.hpp>

namespace KuplungApp {
namespace Utilities {
namespace Export {

ExporterOBJ::~ExporterOBJ() {}

void ExporterOBJ::init(const std::function<void(float)>& doProgress) {
  this->funcProgress = doProgress;
  this->addSuffix = false;
#ifdef _WIN32
  this->nlDelimiter = "\n";
#elif defined macintosh // OS 9
  this->nlDelimiter = "\r";
#else
  this->nlDelimiter = "\n";
#endif

  this->parserUtils = std::make_unique<ParserUtils>();
}

void ExporterOBJ::exportToFile(const FBEntity& file, const std::vector<ModelFaceBase*>& faces, const std::vector<std::string>& settings, std::unique_ptr<ObjectsManager>& managerObjects) {
  this->exportFile = file;
  this->objSettings = settings;
  this->exportGeometry(faces);
  this->exportMaterials(faces);
}

std::string ExporterOBJ::exportMesh(const ModelFaceBase& face) {
  MeshModel model = face.meshModel;
  std::string meshData("");
  std::string v(""), vt(""), vn(""), f("");

  //    int Setting_Axis_Forward = 4;
  //    if (this->objSettings.size() > 0 && !this->objSettings[0].empty())
  //        Setting_Axis_Forward = std::stoi(this->objSettings[0]);
  //    int Setting_Axis_Up = 5;
  //    if (this->objSettings.size() > 1 && !this->objSettings[1].empty())
  //        Setting_Axis_Up = std::stoi(this->objSettings[1]);

  glm::vec3 scale;
  glm::quat rotation;
  glm::vec3 translation;
  glm::vec3 skew;
  glm::vec4 perspective;
  glm::decompose(face.matrixModel, scale, rotation, translation, skew, perspective);

  this->funcProgress(0.0f);
  const int totalProgress = int(model.indices.size()) * 2;
  float progressCounter = 0.0f;

  meshData += this->nlDelimiter;
  meshData += "o " + model.ModelTitle + this->nlDelimiter;
  for (size_t j = 0; j < model.indices.size(); j++) {
    int idx = model.indices[j];
    //glm::vec3 vertex = this->parserUtils->fixVectorAxis(model.vertices[idx], Setting_Axis_Forward, Setting_Axis_Up);
    glm::vec3 vertex = model.vertices[idx];

    vertex += glm::vec3(face.positionX->point, face.positionY->point, face.positionZ->point);
    vertex = vertex * rotation;
    vertex = vertex * scale;

    glm::vec2 texture_coordinate;
    if (model.texture_coordinates.size() > 0)
      texture_coordinate = model.texture_coordinates[idx];
    //glm::vec3 normal = this->parserUtils->fixVectorAxis(model.normals[idx], Setting_Axis_Forward, Setting_Axis_Up);
    glm::vec3 normal = model.normals[idx];

    if (find(this->uniqueVertices.begin(), this->uniqueVertices.end(), vertex) == this->uniqueVertices.end()) {
      this->uniqueVertices.push_back(vertex);
      v += "v " + std::to_string(vertex.x) + " " + std::to_string(vertex.y) + " " + std::to_string(vertex.z) + this->nlDelimiter;
    }

    if (model.texture_coordinates.size() && find(this->uniqueTextureCoordinates.begin(), this->uniqueTextureCoordinates.end(), texture_coordinate) == this->uniqueTextureCoordinates.end()) {
      this->uniqueTextureCoordinates.push_back(texture_coordinate);
      vt += "vt " + std::to_string(texture_coordinate.x) + " " + std::to_string(texture_coordinate.y) + this->nlDelimiter;
    }

    if (find(this->uniqueNormals.begin(), this->uniqueNormals.end(), normal) == this->uniqueNormals.end()) {
      this->uniqueNormals.push_back(normal);
      vn += "vn " + std::to_string(normal.x) + " " + std::to_string(normal.y) + " " + std::to_string(normal.z) + this->nlDelimiter;
    }

    progressCounter += 1;

    this->funcProgress((progressCounter / float(totalProgress)) * 100.0f);
  }

  meshData += v;
  meshData += vt;
  meshData += vn;

  std::string triangleFace("");
  for (size_t k = 0, vCounterT = 1; k < model.indices.size(); k++, vCounterT++) {
    int j = model.indices[int(k)];

    glm::vec3 vertex = model.vertices[j];
    vertex += glm::vec3(face.positionX->point, face.positionY->point, face.positionZ->point);
    vertex = vertex * rotation;
    vertex = vertex * scale;

    long v = find(this->uniqueVertices.begin(), this->uniqueVertices.end(), vertex) - this->uniqueVertices.begin() + 1;
    long vn = find(this->uniqueNormals.begin(), this->uniqueNormals.end(), model.normals[j]) - this->uniqueNormals.begin() + 1;

    long vt = -1;
    if (model.texture_coordinates.size() > 0)
      vt = find(this->uniqueTextureCoordinates.begin(), this->uniqueTextureCoordinates.end(), model.texture_coordinates[j]) - this->uniqueTextureCoordinates.begin();

    triangleFace += " " + std::to_string(v) + "/" + (vt > -1 ? std::to_string(vt + 1) : "") + "/" + std::to_string(vn);

    if (vCounterT % 3 == 0) {
      f += "f" + triangleFace + this->nlDelimiter;
      triangleFace.clear();
    }

    progressCounter += 1;

    this->funcProgress((progressCounter / float(totalProgress)) * 100.0f);
  }

  meshData += "usemtl " + model.MaterialTitle + this->nlDelimiter;
  meshData += "s off" + this->nlDelimiter;
  meshData += f;

  return meshData;
}

void ExporterOBJ::exportGeometry(const std::vector<ModelFaceBase*>& faces) {
  std::string fileContents = "# Kuplung v1.0 OBJ File Export" + this->nlDelimiter;
  fileContents += "# http://www.github.com/supudo/kuplung/" + this->nlDelimiter;
  std::string fn = this->exportFile.title;
  boost::replace_all(fn, ".obj", "");
  fileContents += "mtllib " + fn + ".mtl" + this->nlDelimiter;

  this->uniqueVertices.clear();
  this->uniqueTextureCoordinates.clear();
  this->uniqueNormals.clear();
  this->vCounter = 1;
  this->vtCounter = 1;
  this->vnCounter = 1;

  std::vector<ModelFaceBase*>::const_iterator faceIterator;
  for (faceIterator = faces.begin(); faceIterator != faces.end(); ++faceIterator) {
    fileContents += this->exportMesh(**faceIterator);
  }
  fileContents += this->nlDelimiter;

  if (!fileContents.empty()) {
    time_t t = time(0);
    const struct tm* now = localtime(&t);

    const int year = now->tm_year + 1900;
    const int month = now->tm_mon + 1;
    const int day = now->tm_mday;
    const int hour = now->tm_hour;
    const int minute = now->tm_min;
    const int seconds = now->tm_sec;

    std::string fileSuffix = "_" + std::to_string(year) + std::to_string(month) + std::to_string(day) + std::to_string(hour) + std::to_string(minute) + std::to_string(seconds);

    if (!this->addSuffix)
      fileSuffix.clear();
    std::string filePath = this->exportFile.path.substr(0, this->exportFile.path.find_last_of("\\/"));
    std::string fileName = this->exportFile.title;
    if (boost::algorithm::ends_with(fileName, ".obj"))
      fileName = fileName.substr(0, fileName.size() - 4);
    this->saveFile(fileContents, filePath + "/" + fileName + fileSuffix + ".obj");
  }
}

void ExporterOBJ::exportMaterials(const std::vector<ModelFaceBase*>& faces) {
  std::map<std::string, std::string> materials;
  std::vector<ModelFaceBase*>::const_iterator faceIterator;
  for (faceIterator = faces.begin(); faceIterator != faces.end(); ++faceIterator) {
    MeshModelMaterial mat = (*faceIterator)->meshModel.ModelMaterial;
    if (materials[mat.MaterialTitle].empty()) {
      materials[mat.MaterialTitle] = this->nlDelimiter;
      materials[mat.MaterialTitle] += "newmtl " + mat.MaterialTitle + this->nlDelimiter;
      materials[mat.MaterialTitle] += Settings::Instance()->string_format("Ns %g", mat.SpecularExp) + this->nlDelimiter;
      materials[mat.MaterialTitle] += Settings::Instance()->string_format("Ka %g %g %g", mat.AmbientColor.r, mat.AmbientColor.g, mat.AmbientColor.b) + this->nlDelimiter;
      materials[mat.MaterialTitle] += Settings::Instance()->string_format("Kd %g %g %g", mat.DiffuseColor.r, mat.DiffuseColor.g, mat.DiffuseColor.b) + this->nlDelimiter;
      materials[mat.MaterialTitle] += Settings::Instance()->string_format("Ks %g %g %g", mat.SpecularColor.r, mat.SpecularColor.g, mat.SpecularColor.b) + this->nlDelimiter;
      materials[mat.MaterialTitle] += Settings::Instance()->string_format("Ke %g %g %g", mat.EmissionColor.r, mat.EmissionColor.g, mat.EmissionColor.b) + this->nlDelimiter;
      if (mat.OpticalDensity >= 0.0f)
        materials[mat.MaterialTitle] += Settings::Instance()->string_format("Ni %g", mat.OpticalDensity) + this->nlDelimiter;
      materials[mat.MaterialTitle] += Settings::Instance()->string_format("d %g", mat.Transparency) + this->nlDelimiter;
      materials[mat.MaterialTitle] += Settings::Instance()->string_format("illum %i", mat.IlluminationMode) + this->nlDelimiter;

      if (!mat.TextureAmbient.Image.empty())
        materials[mat.MaterialTitle] += "map_Ka " + mat.TextureAmbient.Image + this->nlDelimiter;
      if (!mat.TextureDiffuse.Image.empty())
        materials[mat.MaterialTitle] += "map_Kd " + mat.TextureDiffuse.Image + this->nlDelimiter;
      if (!mat.TextureDissolve.Image.empty())
        materials[mat.MaterialTitle] += "map_d " + mat.TextureDissolve.Image + this->nlDelimiter;
      if (!mat.TextureBump.Image.empty())
        materials[mat.MaterialTitle] += "map_Bump " + mat.TextureBump.Image + this->nlDelimiter;
      if (!mat.TextureDisplacement.Image.empty())
        materials[mat.MaterialTitle] += "disp " + mat.TextureDisplacement.Image + this->nlDelimiter;
      if (!mat.TextureSpecular.Image.empty())
        materials[mat.MaterialTitle] += "map_Ks " + mat.TextureSpecular.Image + this->nlDelimiter;
      if (!mat.TextureSpecularExp.Image.empty())
        materials[mat.MaterialTitle] += "map_Ns " + mat.TextureSpecularExp.Image + this->nlDelimiter;
    }
  }

  std::string fileContents = "# Kuplung MTL File" + this->nlDelimiter;
  fileContents += "# Material Count: " + std::to_string(materials.size()) + this->nlDelimiter;
  fileContents += "# http://www.github.com/supudo/kuplung/" + this->nlDelimiter;

  for (std::map<std::string, std::string>::iterator iter = materials.begin(); iter != materials.end(); ++iter) {
    fileContents += iter->second;
  }

  fileContents += this->nlDelimiter;

  if (!fileContents.empty()) {
    std::string filePath = this->exportFile.path.substr(0, this->exportFile.path.find_last_of("\\/"));
    std::string fileName = this->exportFile.title;
    if (boost::algorithm::ends_with(fileName, ".obj"))
      fileName = fileName.substr(0, fileName.size() - 4);
    this->saveFile(fileContents, filePath + "/" + fileName + ".mtl");
  }
}

//void ExporterOBJ::exportMaterials(const std::vector<ModelFaceBase*>& faces) {
//    std::map<std::string, std::string> materials;
//    for (int i=0; i<(int)faces.size(); i++) {
//        MeshModelMaterial mat = faces[i]->meshModel.ModelMaterial;
//        if (materials[mat.MaterialTitle].empty()) {
//            materials[mat.MaterialTitle] = this->nlDelimiter;
//            materials[mat.MaterialTitle] += "newmtl " + mat.MaterialTitle + this->nlDelimiter;
//            materials[mat.MaterialTitle] += Settings::Instance()->string_format("Ns %f", mat.SpecularExp) + this->nlDelimiter;
//            materials[mat.MaterialTitle] += Settings::Instance()->string_format("Ka %f %f %f", mat.AmbientColor.r, mat.AmbientColor.g, mat.AmbientColor.b) + this->nlDelimiter;
//            materials[mat.MaterialTitle] += Settings::Instance()->string_format("Kd %f %f %f", mat.DiffuseColor.r, mat.DiffuseColor.g, mat.DiffuseColor.b) + this->nlDelimiter;
//            materials[mat.MaterialTitle] += Settings::Instance()->string_format("Ks %f %f %f", mat.SpecularColor.r, mat.SpecularColor.g, mat.SpecularColor.b) + this->nlDelimiter;
//            materials[mat.MaterialTitle] += Settings::Instance()->string_format("Ke %f %f %f", mat.EmissionColor.r, mat.EmissionColor.g, mat.EmissionColor.b) + this->nlDelimiter;
//            if (mat.OpticalDensity >= 0.0f)
//                materials[mat.MaterialTitle] += Settings::Instance()->string_format("Ni %f", mat.OpticalDensity) + this->nlDelimiter;
//            materials[mat.MaterialTitle] += Settings::Instance()->string_format("d %f", mat.Transparency) + this->nlDelimiter;
//            materials[mat.MaterialTitle] += Settings::Instance()->string_format("illum %i", mat.IlluminationMode) + this->nlDelimiter;

//            if (!mat.TextureAmbient.Image.empty())
//                materials[mat.MaterialTitle] += "map_Ka " + mat.TextureAmbient.Image + this->nlDelimiter;
//            if (!mat.TextureDiffuse.Image.empty())
//                materials[mat.MaterialTitle] += "map_Kd " + mat.TextureDiffuse.Image + this->nlDelimiter;
//            if (!mat.TextureDissolve.Image.empty())
//                materials[mat.MaterialTitle] += "map_d " + mat.TextureDissolve.Image + this->nlDelimiter;
//            if (!mat.TextureBump.Image.empty())
//                materials[mat.MaterialTitle] += "map_Bump " + mat.TextureBump.Image + this->nlDelimiter;
//            if (!mat.TextureDisplacement.Image.empty())
//                materials[mat.MaterialTitle] += "disp " + mat.TextureDisplacement.Image + this->nlDelimiter;
//            if (!mat.TextureSpecular.Image.empty())
//                materials[mat.MaterialTitle] += "map_Ks " + mat.TextureSpecular.Image + this->nlDelimiter;
//            if (!mat.TextureSpecularExp.Image.empty())
//                materials[mat.MaterialTitle] += "map_Ns " + mat.TextureSpecularExp.Image + this->nlDelimiter;
//        }
//    }

//    std::string fileContents = "# Kuplung MTL File" + this->nlDelimiter;
//    fileContents += "# Material Count: " + std::to_string(materials.size()) + this->nlDelimiter;
//    fileContents += "# http://www.github.com/supudo/kuplung/" + this->nlDelimiter;

//    for (std::map<std::string, std::string>::iterator iter = materials.begin(); iter != materials.end(); ++iter) {
//        fileContents += iter->second;
//    }

//    fileContents += this->nlDelimiter;

//    if (!fileContents.empty()) {
//        std::string filePath = this->exportFile.path.substr(0, this->exportFile.path.find_last_of("\\/"));
//        std::string fileName = this->exportFile.title;
//        if (boost::algorithm::ends_with(fileName, ".obj"))
//            fileName = fileName.substr(0, fileName.size() - 4);
//        this->saveFile(fileContents, filePath + "/" + fileName + ".mtl");
//    }
//}

void ExporterOBJ::saveFile(const std::string& fileContents, const std::string& fileName) const {
  //    printf("--------------------------------------------------------\n");
  //    printf("%s\n", fileName.c_str());
  //    printf("%s\n", fileContents.c_str());
  //    printf("--------------------------------------------------------\n");

  std::ofstream out(fileName);
  if (out.is_open()) {
    out << fileContents;
    out.close();
  }
}

} // namespace Export
} // namespace Utilities
} // namespace KuplungApp


#include "../chainparams.h"
#include "../lelantus.h"
#include "../script/standard.h"
#include "../validation.h"
#include "../wallet/coincontrol.h"
#include "../wallet/wallet.h"

#include "test_bitcoin.h"
#include "fixtures.h"

#include <boost/test/unit_test.hpp>

namespace lelantus {

struct JoinSplitScriptGenerator {
    Params const *params;
    std::vector<std::pair<PrivateCoin, uint32_t>> coins;
    std::map<uint32_t, std::vector<PublicCoin>> anons;
    CAmount vout;
    std::vector<PrivateCoin> coinsOut;
    CAmount fee;
    std::map<uint32_t, uint256> groupBlockHashes;
    uint256 txHash;

    std::pair<CScript, JoinSplit> Get() {
        // auto p = params ? params : Params::get_default();
        auto p = Params::get_default();

        CScript script;

        JoinSplit joinSplit(p, coins, anons, vout, coinsOut, fee, groupBlockHashes, txHash);
        joinSplit.setVersion(LELANTUS_TX_VERSION_4);

        CDataStream ss(SER_NETWORK, PROTOCOL_VERSION);
        ss << joinSplit;

        script << OP_LELANTUSJOINSPLIT;
        script.insert(script.end(), ss.begin(), ss.end());

        return {script, joinSplit};
    }
};

class LelantusTests : public LelantusTestingSetup {
public:
    LelantusTests() :
        LelantusTestingSetup(),
        lelantusState(CLelantusState::GetState()),
        consensus(::Params().GetConsensus()) {
    }

    ~LelantusTests() {
        lelantusState->Reset();
    }

public:

    std::vector<PublicCoin> ExtractCoins(std::vector<PrivateCoin> const &coins) {
        std::vector<PublicCoin> pubs;
        pubs.reserve(coins.size());

        for (auto &c : coins) {
            pubs.push_back(c.getPublicCoin());
        }

        return pubs;
    }

    void ExtractJoinSplit(CMutableTransaction const &tx,
        std::vector<PublicCoin> &newMints,
        std::vector<Scalar> &serials) {
        for (auto const &in : tx.vin) {
            if (in.IsLelantusJoinSplit()) {
                auto js = ParseLelantusJoinSplit(in);
                auto const &s = js->getCoinSerialNumbers();
                serials.insert(serials.end(), s.begin(), s.end());
            }
        }

        for (auto const &out : tx.vout) {
            if (out.scriptPubKey.IsLelantusJMint()) {
                GroupElement coin;
                ParseLelantusMintScript(out.scriptPubKey, coin);

                newMints.push_back(coin);
            }
        }
    }

    CBlock GetCBlock(CBlockIndex const *blockIdx) {
        CBlock block;
        if (!ReadBlockFromDisk(block, blockIdx, ::Params().GetConsensus())) {
            throw std::invalid_argument("No block index data");
        }

        return block;
    }

    void PopulateLelantusTxInfo(
        CBlock &block,
        std::vector<std::pair<lelantus::PublicCoin, std::pair<uint64_t, uint256>>> const &mints,
        std::vector<std::pair<Scalar, int>> const &serials) {
        block.lelantusTxInfo = std::make_shared<lelantus::CLelantusTxInfo>();
        block.lelantusTxInfo->mints.insert(block.lelantusTxInfo->mints.end(), mints.begin(), mints.end());

        for (auto const &s : serials) {
            block.lelantusTxInfo->spentSerials.emplace(s);
        }

        block.lelantusTxInfo->Complete();
    }

    CTransaction GenerateJoinSplit(
        std::vector<CAmount> const &outs,
        std::vector<CAmount> const &mints,
        CCoinControl const *coinControl = nullptr) {

        std::vector<CRecipient> vecs;
        for (auto const &out : outs) {
            LOCK(pwalletMain->cs_wallet);
            auto pub = pwalletMain->GenerateNewKey();

            vecs.push_back(
            {
                GetScriptForDestination(pub.GetID()),
                out,
                false
            });
        }

        std::vector<CLelantusEntry>  spendCoins;
        std::vector<CHDMint> mintCoins;

        CAmount fee;
        auto result = pwalletMain->CreateLelantusJoinSplitTransaction(
            vecs, fee, mints, spendCoins, mintCoins, coinControl);

        if (!pwalletMain->CommitLelantusTransaction(
            result, spendCoins, mintCoins)) {
            throw std::runtime_error("Fail to commit transaction");
        }

        return result;
    }

public:
    CLelantusState *lelantusState;
    Consensus::Params const &consensus;
};

BOOST_FIXTURE_TEST_SUITE(lelantus_tests, LelantusTests)

BOOST_AUTO_TEST_CASE(schnorr_proof)
{
    auto params = Params::get_default();

    PrivateCoin coin(params, 1);

    CDataStream  serializedSchnorrProof(SER_NETWORK, PROTOCOL_VERSION);
    GenerateMintSchnorrProof(coin, serializedSchnorrProof);

    auto commitment = coin.getPublicCoin();
    SchnorrProof proof;
    serializedSchnorrProof >> proof;

    BOOST_CHECK(VerifyMintSchnorrProof(1, commitment.getValue(), proof));
}

BOOST_AUTO_TEST_CASE(is_lelantus_allowed)
{
    auto start = ::Params().GetConsensus().nLelantusStartBlock;
    BOOST_CHECK(!IsLelantusAllowed(0));
    BOOST_CHECK(!IsLelantusAllowed(start - 1));
    BOOST_CHECK(IsLelantusAllowed(start));
    BOOST_CHECK(IsLelantusAllowed(start + 1));
}

BOOST_AUTO_TEST_CASE(parse_lelantus_mintscript)
{
    // payload: op_code + pubcoin + schnorrproof
    PrivateCoin priv(params, 1);
    auto &pub = priv.getPublicCoin();

    CDataStream  proofSerialized(SER_NETWORK, PROTOCOL_VERSION);

    GenerateMintSchnorrProof(priv, proofSerialized);

    CScript script(OP_LELANTUSMINT);

    auto vch = pub.getValue().getvch();
    script.insert(script.end(), vch.begin(), vch.end());
    script.insert(script.end(), proofSerialized.begin(), proofSerialized.end());

    // verify
    secp_primitives::GroupElement parsedCoin;
    ParseLelantusMintScript(script, parsedCoin);

    BOOST_CHECK(pub.getValue() == parsedCoin);

    SchnorrProof proof;
    uint256 mintTag;
    ParseLelantusMintScript(script, parsedCoin, proof, mintTag);

    BOOST_CHECK(pub.getValue() == parsedCoin);
    BOOST_CHECK(VerifyMintSchnorrProof(1, parsedCoin, proof));

    CDataStream  parsedProof(SER_NETWORK, PROTOCOL_VERSION);
    parsedProof << proof;

    BOOST_CHECK(proofSerialized.vch == parsedProof.vch);

    GroupElement parsedCoin2;
    ParseLelantusMintScript(script, parsedCoin2);

    BOOST_CHECK(pub.getValue() == parsedCoin2);

    script.resize(script.size() - 1);
    BOOST_CHECK_THROW(ParseLelantusMintScript(script, parsedCoin, proof, mintTag), std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(parse_lelantus_jmint)
{
    GroupElement val;
    val.randomize();

    CScript script(OP_LELANTUSJMINT);

    auto vch = val.getvch();
    script.insert(script.end(), vch.begin(), vch.end());

    std::vector<unsigned char> encrypted;
    encrypted.resize(16);

    std::fill(encrypted.begin(), encrypted.end(), 0xff);
    script.insert(script.end(), encrypted.begin(), encrypted.end());

    // parse and verify
    GroupElement outCoin;
    std::vector<unsigned char> outEnc;
    ParseLelantusJMintScript(script, outCoin, outEnc);

    BOOST_CHECK(val == outCoin);
    BOOST_CHECK(encrypted == outEnc);

    GroupElement outCoin2;
    ParseLelantusMintScript(script, outCoin2);

    BOOST_CHECK(val == outCoin2);

    // parse invalid
    script.resize(script.size() - 1);
    BOOST_CHECK_THROW(ParseLelantusJMintScript(script, outCoin, outEnc), std::invalid_argument);
}

BOOST_AUTO_TEST_CASE(get_outpoint)
{
    GenerateBlocks(110);

    // generate mints
    std::vector<CMutableTransaction> txs;
    auto mints = GenerateMints({2, 10}, txs);
    auto mint = mints[0];
    auto nonCommitted = mints[1];
    auto tx = txs[0];
    size_t mintIdx = 0;
    for (; mintIdx < tx.vout.size(); mintIdx++) {
        if (tx.vout[mintIdx].scriptPubKey.IsLelantusMint()) {
            break;
        }
    }

    auto blockIdx = GenerateBlock({txs[0]});

    CBlock block;
    BOOST_CHECK(ReadBlockFromDisk(block, blockIdx, ::Params().GetConsensus()));

    block.lelantusTxInfo = std::make_shared<lelantus::CLelantusTxInfo>();
    block.lelantusTxInfo->mints.emplace_back(std::make_pair(mint.GetPubcoinValue(), std::make_pair(mint.GetAmount(), uint256())));

    lelantusState->AddMintsToStateAndBlockIndex(blockIdx, &block);
    lelantusState->AddBlock(blockIdx);

    // verify
    COutPoint expectedOut(tx.GetHash(), mintIdx);

    // GetOutPointFromBlock
    COutPoint out;
    BOOST_CHECK(GetOutPointFromBlock(out, mint.GetPubcoinValue(), block));
    BOOST_CHECK(expectedOut == out);

    BOOST_CHECK(!GetOutPointFromBlock(out, nonCommitted.GetPubcoinValue(), block));

    // GetOutPoint
    // by pubcoin
    out = COutPoint();
    BOOST_CHECK(GetOutPoint(out, PublicCoin(mint.GetPubcoinValue())));
    BOOST_CHECK(expectedOut == out);

    BOOST_CHECK(!GetOutPoint(out, PublicCoin(nonCommitted.GetPubcoinValue())));

    // by pubcoin value
    out = COutPoint();
    BOOST_CHECK(GetOutPoint(out, mint.GetPubcoinValue()));
    BOOST_CHECK(expectedOut == out);

    BOOST_CHECK(!GetOutPoint(out, nonCommitted.GetPubcoinValue()));

    // by pubcoin hash
    out = COutPoint();
    BOOST_CHECK(GetOutPoint(out, mint.GetPubCoinHash()));
    BOOST_CHECK(expectedOut == out);

    BOOST_CHECK(!GetOutPoint(out, nonCommitted.GetPubCoinHash()));
}

BOOST_AUTO_TEST_CASE(build_lelantus_state)
{
    GenerateBlocks(110);

    // generate mints
    std::vector<CMutableTransaction> txs;
    auto mints = GenerateMints({1 * COIN, 2 * COIN, 10 * COIN, 100 * COIN}, txs);

    GenerateBlock({txs[0], txs[1]});
    auto blockIdx1 = chainActive.Tip();
    auto block1 = GetCBlock(blockIdx1);

    GenerateBlock({txs[2], txs[3]});
    auto blockIdx2 = chainActive.Tip();
    auto block2 = GetCBlock(blockIdx2);

    block1.lelantusTxInfo = std::make_shared<lelantus::CLelantusTxInfo>();
    block2.lelantusTxInfo = std::make_shared<lelantus::CLelantusTxInfo>();

    block1.lelantusTxInfo->mints.emplace_back(std::make_pair(mints[0].GetPubcoinValue(), std::make_pair(mints[0].GetAmount(), uint256())));
    block1.lelantusTxInfo->mints.emplace_back(std::make_pair(mints[1].GetPubcoinValue(), std::make_pair(mints[1].GetAmount(), uint256())));
    block1.lelantusTxInfo->mints.emplace_back(std::make_pair(mints[2].GetPubcoinValue(), std::make_pair(mints[2].GetAmount(), uint256())));
    block1.lelantusTxInfo->mints.emplace_back(std::make_pair(mints[3].GetPubcoinValue(), std::make_pair(mints[3].GetAmount(), uint256())));

    lelantusState->AddMintsToStateAndBlockIndex(blockIdx1, &block1);
    lelantusState->AddMintsToStateAndBlockIndex(blockIdx2, &block2);

    BOOST_CHECK(BuildLelantusStateFromIndex(&chainActive));
    BOOST_CHECK(lelantusState->HasCoin(mints[0].GetPubcoinValue()));
    BOOST_CHECK(lelantusState->HasCoin(mints[1].GetPubcoinValue()));
    BOOST_CHECK(lelantusState->HasCoin(mints[2].GetPubcoinValue()));
    BOOST_CHECK(lelantusState->HasCoin(mints[3].GetPubcoinValue()));
}

BOOST_AUTO_TEST_CASE(connect_and_disconnect_block)
{
    // util function
    auto reconnect = [](CBlock const &block) {
        LOCK(cs_main);

        std::shared_ptr<CBlock const> sharedBlock =
        std::make_shared<CBlock const>(block);

        CValidationState state;
        ActivateBestChain(state, ::Params(), sharedBlock);
    };

    GenerateBlocks(1000);

    std::vector<CMutableTransaction> mintTxs;
    auto hdMints = GenerateMints({3 * COIN, 3 * COIN, 3 * COIN}, mintTxs);

    struct {
        // expected state.
        std::vector<PublicCoin> coins;
        std::vector<Scalar> serials;

        // first group
        CBlockIndex *first = nullptr;
        CBlockIndex *last = nullptr;

        int lastId = 0;

        // real state
        CLelantusState *state;

        void Verify() const {
            auto const &spends = state->GetSpends();
            BOOST_CHECK_EQUAL(serials.size(), spends.size());
            for (auto const &s : serials) {
                BOOST_CHECK_MESSAGE(spends.count(s), "serial is not found on state");
            }

            auto const &mints = state->GetMints();
            BOOST_CHECK_EQUAL(coins.size(), mints.size());
            for (auto const &c : coins) {
                BOOST_CHECK_MESSAGE(mints.count(c), "public is not found on state");
            }

            auto retrievedId = state->GetLatestCoinID();

            CLelantusState::LelantusCoinGroupInfo group;
            state->GetCoinGroupInfo(retrievedId, group);
            BOOST_CHECK_EQUAL(lastId, retrievedId);
            BOOST_CHECK_EQUAL(first, group.firstBlock);
            BOOST_CHECK_EQUAL(last, group.lastBlock);
            BOOST_CHECK_EQUAL(coins.size(), group.nCoins);
        }
    } checker;
    checker.state = lelantusState;

    // Cache empty checker
    auto emptyChecker = checker;

    // Generate some txs which contain mints
    auto blockIdx1 = GenerateBlock({mintTxs[0], mintTxs[1]});
    BOOST_CHECK(blockIdx1);
    auto block1 = GetCBlock(blockIdx1);

    checker.coins.push_back(hdMints[0].GetPubcoinValue());
    checker.coins.push_back(hdMints[1].GetPubcoinValue());
    checker.first = blockIdx1;
    checker.last = blockIdx1;
    checker.lastId = 1;
    checker.Verify();

    // Generate empty blocks should not effect state
    GenerateBlocks(10);
    checker.Verify();

    // Add spend tx

    // Create two txs which contains same serial.
    CCoinControl coinControl;

    {
        auto tx = mintTxs[0];
        auto it = std::find_if(tx.vout.begin(), tx.vout.end(), [](CTxOut const &out) -> bool {
            return out.scriptPubKey.IsLelantusMint();
        });
        BOOST_CHECK(it != tx.vout.end());

        coinControl.Select(COutPoint(tx.GetHash(), std::distance(tx.vout.begin(), it)));
    }

    auto jsTx1 = GenerateJoinSplit({1 * COIN}, {}, &coinControl);

    // Update isused status
    {
        auto mint = hdMints[0];
        auto hash = primitives::GetPubCoinValueHash(mint.GetPubcoinValue());

        CLelantusMintMeta meta;
        BOOST_CHECK(pwalletMain->zwallet->GetTracker()
            .GetLelantusMetaFromPubcoin(hash, meta));

        BOOST_CHECK(meta.isUsed);

        meta.isUsed = false;
        BOOST_CHECK(pwalletMain->zwallet->GetTracker().UpdateState(meta));

        meta = CLelantusMintMeta();
        BOOST_CHECK(pwalletMain->zwallet->GetTracker()
            .GetLelantusMetaFromPubcoin(hash, meta));
        BOOST_CHECK(!meta.isUsed);
    }

    // Create duplicated serial tx and test this at the bottom
    auto dupJsTx1 = GenerateJoinSplit({1 * COIN}, {}, &coinControl);

    std::vector<PublicCoin> dupNewCoins1;
    std::vector<Scalar> dupSerials1;
    ExtractJoinSplit(dupJsTx1, dupNewCoins1, dupSerials1);

    std::vector<PublicCoin> newCoins1;
    std::vector<Scalar> serials1;
    ExtractJoinSplit(jsTx1, newCoins1, serials1);
    BOOST_CHECK_EQUAL(1, newCoins1.size());
    BOOST_CHECK_EQUAL(1, serials1.size());
    BOOST_CHECK(dupSerials1[0] == serials1[0]);

    auto blockIdx2 = GenerateBlock({jsTx1});
    BOOST_CHECK(blockIdx2);
    auto block2 = GetCBlock(blockIdx2);

    auto cacheChecker = checker;
    checker.coins.push_back(newCoins1.front());
    checker.serials.push_back(serials1.front());
    checker.last = blockIdx2;

    checker.Verify();

    // state should be rolled back
    DisconnectBlocks(1);
    BOOST_CHECK_EQUAL(chainActive.Tip()->nHeight, blockIdx2->nHeight - 1);
    cacheChecker.Verify();

    // reconnect
    reconnect(block2);
    checker.Verify();

    // add more block contain both mint and serial
    auto jsTx2 = GenerateJoinSplit({1 * COIN}, {CENT});
    std::vector<PublicCoin> newCoins2;
    std::vector<Scalar> serials2;
    ExtractJoinSplit(jsTx2, newCoins2, serials2);
    BOOST_CHECK_EQUAL(2, newCoins2.size());
    BOOST_CHECK_EQUAL(1, serials2.size());

    auto blockIdx3 = GenerateBlock({mintTxs[2], jsTx2});
    BOOST_CHECK(blockIdx3);
    auto block3 = GetCBlock(blockIdx3);

    checker.coins.insert(checker.coins.end(), newCoins2.begin(), newCoins2.end());
    checker.coins.push_back(hdMints[2].GetPubcoinValue());
    checker.serials.push_back(serials2[0]);
    checker.last = blockIdx3;

    checker.Verify();

    // Clear state and rebuild
    lelantusState->Reset();
    emptyChecker.Verify();

    BuildLelantusStateFromIndex(&chainActive);
    checker.Verify();

    // Disconnect all and reconnect
    std::vector<CBlock> blocks;
    while (chainActive.Tip() != chainActive.Genesis()) {
        blocks.push_back(GetCBlock(chainActive.Tip()));
        DisconnectBlocks(1);
    }

    emptyChecker.Verify();

    for (auto const &block : blocks) {
        reconnect(block);
    }

    checker.Verify();

    // double spend
    auto currentBlock = chainActive.Tip()->nHeight;
    BOOST_CHECK(!GenerateBlock({dupJsTx1}));
    BOOST_CHECK_EQUAL(currentBlock, chainActive.Tip()->nHeight);
    mempool.clear();
    lelantusState->Reset();
}

BOOST_AUTO_TEST_CASE(checktransaction)
{
    GenerateBlocks(1000);

    // mints
    std::vector<CMutableTransaction> txs;
    auto mints = GenerateMints({1 * CENT}, txs);
    auto &tx = txs[0];

    CValidationState state;
    CLelantusTxInfo info;
    BOOST_CHECK(CheckLelantusTransaction(
        txs[0], state, tx.GetHash(), true, chainActive.Height(), true, true, NULL, &info));

    std::vector<std::pair<PublicCoin, std::pair<uint64_t, uint256>>> expectedCoins = {{mints[0].GetPubcoinValue(), {1 * CENT, info.mints[0].second.second}}};

    BOOST_CHECK(expectedCoins == info.mints);

    // join split
    txs.clear();
    mints = GenerateMints({10 * CENT, 11 * CENT, 100 * CENT}, txs);
    GenerateBlock(txs);
    GenerateBlocks(10);

    auto outputAmount = 8 * CENT;
    auto mintAmount = 2 * CENT - CENT; // a cent as fee

    CWalletTx wtx;
    pwalletMain->JoinSplitLelantus(
        {{script, outputAmount, false}},
        {mintAmount},
        wtx);

    CMutableTransaction joinsplitTx(wtx);
    auto joinsplit = ParseLelantusJoinSplit(joinsplitTx.vin[0]);

    // test get join split amounts
    BOOST_CHECK_EQUAL(1, GetSpendInputs(joinsplitTx));
    BOOST_CHECK_EQUAL(1, GetSpendInputs(joinsplitTx, joinsplitTx.vin[0]));

    info = CLelantusTxInfo();

    BOOST_CHECK(CheckLelantusTransaction(
        joinsplitTx, state, joinsplitTx.GetHash(), false, chainActive.Height(), false, true, NULL, &info));

    auto &serials = joinsplit->getCoinSerialNumbers();
    auto &ids = joinsplit->getCoinGroupIds();

    for (size_t i = 0; i != serials.size(); i++) {
        bool hasSerial = false;
        BOOST_CHECK_MESSAGE(hasSerial = (info.spentSerials.count(serials[i]) > 0), "No serial as expected");
        if (hasSerial) {
            BOOST_CHECK_MESSAGE(ids[i] == info.spentSerials[serials[i]], "Serials group id is invalid");
        }
    }

    info = CLelantusTxInfo();
    BOOST_CHECK(CheckLelantusTransaction(
        joinsplitTx, state, joinsplitTx.GetHash(), false, INT_MAX, false, true, NULL, &info));

    // test surge dection.
    while (!lelantusState->IsSurgeConditionDetected()) {
        Scalar s;
        s.randomize();

        lelantusState->AddSpend(s, 1);
    }

    BOOST_CHECK(!CheckLelantusTransaction(
        joinsplitTx, state, joinsplitTx.GetHash(), false, INT_MAX, false, true, NULL, &info));
}

BOOST_AUTO_TEST_CASE(spend_limitation_per_tx)
{
    PrivateCoin coinOut(params, 0);
    JoinSplitScriptGenerator invalidG, validG;
    invalidG.fee = 0;
    invalidG.vout = 0;
    invalidG.coinsOut = {coinOut};
    invalidG.groupBlockHashes[1] = {ArithToUint256(0)};
    invalidG.txHash = ArithToUint256(0);

    validG.fee = 0;
    validG.vout = 0;
    validG.coinsOut = {coinOut};
    validG.groupBlockHashes[1] = ArithToUint256(0);
    validG.txHash = ArithToUint256(0);

    for (size_t i = 0; i != consensus.nMaxLelantusInputPerTransaction + 1; i++) {
        PrivateCoin coin(params, 0);
        invalidG.coins.emplace_back(coin, 1);
        invalidG.anons[1].push_back(coin.getPublicCoin());

        if (i != 0) { // skip first
            validG.coins.emplace_back(coin, 1);
            validG.anons[1].push_back(coin.getPublicCoin());
        }
    }

    CMutableTransaction invalidTx, validTx;
    invalidTx.vin.resize(1);
    invalidTx.vin[0].scriptSig = invalidG.Get().first;

    validTx.vin.resize(1);
    validTx.vin[0].scriptSig = validG.Get().first;

    CBlock invalidBlock, validBlock;
    invalidBlock.vtx.push_back(MakeTransactionRef(invalidTx));
    validBlock.vtx.push_back(MakeTransactionRef(validTx));

    CValidationState state;
    BOOST_CHECK(!CheckLelantusBlock(state, invalidBlock));
    BOOST_CHECK(CheckLelantusBlock(state, validBlock));
}

BOOST_AUTO_TEST_CASE(spend_limitation_per_block)
{
    CBlock block;
    size_t spends = 0;

    for (size_t i = 0; spends <= consensus.nMaxLelantusInputPerBlock; i++) {
        PrivateCoin coinOut(params, 0);
        JoinSplitScriptGenerator g;
        g.fee = 0;
        g.vout = 0;
        g.coinsOut = {coinOut};
        for (size_t i = 0; i != consensus.nMaxLelantusInputPerTransaction; i++) {
            PrivateCoin coin(params, 0);
            g.coins.emplace_back(coin, 1);
            g.anons[1].push_back(coin.getPublicCoin());

            spends++;
        }

        g.groupBlockHashes[1] = ArithToUint256(i);
        g.txHash = ArithToUint256(i);

        CMutableTransaction tx;
        tx.vin.resize(1);
        tx.vin[0].scriptSig = g.Get().first;

        block.vtx.push_back(MakeTransactionRef(tx));
    }

    CValidationState state;
    BOOST_CHECK(!CheckLelantusBlock(state, block));

    block.vtx.pop_back();
    BOOST_CHECK(CheckLelantusBlock(state, block));
}

BOOST_AUTO_TEST_CASE(parse_joinsplit)
{
    auto coins = GenerateMints({1 * COIN, 10 * COIN, 1 * COIN, 1 * COIN});

    JoinSplitScriptGenerator g;
    g.params = params;
    g.coins = {{coins[0], 1}, {coins[1], 1}, {coins[2], 2}};
    for (auto id : {1, 2}) {
        for (size_t i = 0; i != 10; i++) {
            GroupElement e;
            e.randomize();

            g.anons[id].emplace_back(e);
        }
    }

    g.anons[1][0] = coins[0].getPublicCoin();
    g.anons[1][1] = coins[1].getPublicCoin();
    g.anons[2][0] = coins[2].getPublicCoin();

    g.vout = 11 * COIN - CENT;
    g.coinsOut.push_back(coins[3]);
    g.fee = CENT;
    g.groupBlockHashes[1] = ArithToUint256(1);
    g.groupBlockHashes[2] = ArithToUint256(2);
    g.txHash = ArithToUint256(3);

    auto gs = g.Get();
    CTxIn inp(COutPoint(), gs.first);

    auto result = ParseLelantusJoinSplit(inp);

    BOOST_CHECK(gs.second.getCoinSerialNumbers() == result->getCoinSerialNumbers());
    BOOST_CHECK(gs.second.getFee() == result->getFee());
    BOOST_CHECK(gs.second.getCoinGroupIds() == result->getCoinGroupIds());
    BOOST_CHECK(gs.second.getIdAndBlockHashes() == result->getIdAndBlockHashes());
    BOOST_CHECK(gs.second.getVersion() == result->getVersion());
    BOOST_CHECK(gs.second.HasValidSerials() == result->HasValidSerials());

    BOOST_CHECK(gs.second.Verify(g.anons, ExtractCoins(g.coinsOut), g.vout, g.txHash));
    BOOST_CHECK(result->Verify(g.anons, ExtractCoins(g.coinsOut), g.vout, g.txHash));
}

BOOST_AUTO_TEST_CASE(coingroup)
{
    GenerateBlocks(1000);

    // util function
    auto reconnect = [](CBlock const &block) {
        LOCK2(cs_main, pwalletMain->cs_wallet);
        LOCK(mempool.cs);

        std::shared_ptr<CBlock const> sharedBlock =
        std::make_shared<CBlock const>(block);

        CValidationState state;
        ActivateBestChain(state, ::Params(), sharedBlock);
    };

    struct {
        // expected state.
        std::vector<PublicCoin> coins;

        // first group
        CBlockIndex *first = nullptr;
        CBlockIndex *last = nullptr;

        int lastId = 0;
        size_t lastGroupCoins = 0;

        // real state
        CLelantusState *state;

        void Verify(std::string stateName = "") const {
            auto const &mints = state->GetMints();
            BOOST_CHECK_EQUAL(coins.size(), mints.size());
            for (auto const &c : coins) {
                BOOST_CHECK_MESSAGE(mints.count(c), "public is not found on state : " + stateName);
            }

            auto retrievedId = state->GetLatestCoinID();

            CLelantusState::LelantusCoinGroupInfo group;
            state->GetCoinGroupInfo(retrievedId, group);

            BOOST_CHECK_EQUAL(lastId, retrievedId);
            BOOST_CHECK_EQUAL(first, group.firstBlock);
            BOOST_CHECK_EQUAL(last, group.lastBlock);
            BOOST_CHECK_EQUAL(lastGroupCoins, group.nCoins);
        }
    } checker;
    checker.state = lelantusState;

    lelantusState->~CLelantusState();
    new (lelantusState) CLelantusState(65, 16);
    lelantusState->Reset();

    // logic
    std::vector<CMutableTransaction> txs;
    std::vector<lelantus::PrivateCoin> coins;
    auto hdMints = GenerateMints(std::vector<CAmount>(66, 1), txs, coins);

    auto txRange = [&](size_t start, size_t end) -> std::vector<CMutableTransaction> {
        std::vector<CMutableTransaction> rangeTxs;
        for (auto i = start; i < end && i < txs.size(); i++) {
            rangeTxs.push_back(txs[i]);
        }

        return rangeTxs;
    };

    std::vector<PublicCoin> pubCoins;
    for (auto const &hdMint : hdMints) {
        pubCoins.push_back(hdMint.GetPubcoinValue());
    }

    auto emptyChecker = checker;
    emptyChecker.Verify();

    // add one block
    auto idx1 = GenerateBlock(txRange(0, 1));
    auto block1 = GetCBlock(idx1);

    checker.coins.push_back(pubCoins[0]);
    checker.lastId = 1;
    checker.first = idx1;
    checker.last = idx1;
    checker.lastGroupCoins = 1;
    checker.Verify();

    // add more
    auto idx2 = GenerateBlock(txRange(1, 32));
    auto block2 = GetCBlock(idx2);

    checker.coins.insert(checker.coins.end(), pubCoins.begin() + 1, pubCoins.begin() + 32);
    checker.last = idx2;
    checker.lastGroupCoins = 32;
    checker.Verify();

    auto cacheIdx2Checker = checker;

    // add more to fill group
    auto idx3 = GenerateBlock(txRange(32, 65));
    auto block3 = GetCBlock(idx3);

    checker.coins.insert(checker.coins.end(), pubCoins.begin() + 32, pubCoins.begin() + 65);
    checker.last = idx3;
    checker.lastGroupCoins = 65;
    checker.Verify();

    auto cacheIdx3Checker = checker;

    // add one more to create new group
    auto idx4 = GenerateBlock(txRange(65, 66));
    auto block4 = GetCBlock(idx4);

    checker.coins.push_back(pubCoins[65]);
    checker.lastId = 2;
    checker.lastGroupCoins = 34;
    checker.first = idx3;
    checker.last = idx4;

    checker.Verify();

    // remove last block check coingroup
    DisconnectBlocks(1);
    cacheIdx3Checker.Verify();

    // remove one more block
    DisconnectBlocks(1);
    cacheIdx2Checker.Verify();

    // reconnect them all and check state
    reconnect(block2);
    reconnect(block3);
    checker.Verify();

    lelantusState->~CLelantusState();
    new (lelantusState) CLelantusState();
    lelantusState->Reset();
}

BOOST_AUTO_TEST_SUITE_END()

};


#include "Time.h"
#include "TimeScale.h"

#include <boost/date_time/posix_time/posix_time.hpp>
using boost::posix_time::ptime;
using boost::posix_time::time_duration;
using boost::gregorian::date;
using boost::gregorian::days;

namespace teatime {

const int64 Epoch1400 = 510974; // number of days between 1.1.1 and 1.1.1400
const int64 Epoch1970 = 719162; // number of days between 1.1.1 and 1.1.1970

Time::Time(int64 ticks)
{
  this->ticks_ = ticks;
}

Time::Time(ptime t)
{
  auto datePart = t.date();
  auto timePart = t.time_of_day();

  TimeScale timeScale = TimeScale::Master();

  // consider date part only
  auto d = datePart - date(1970, 1, 1);
  int offsetFromEpochDays = d.days();
  this->ticks_ = timeScale.TicksPerDay() * offsetFromEpochDays;

  // add ticks for time of day
  if(time_duration::ticks_per_second() > timeScale.TicksPerSecond()) // since the following computations are integer calculcations, we distinguish here
  {
    int64 conversionRate = time_duration::ticks_per_second() / timeScale.TicksPerSecond();
    this->ticks_ += timePart.ticks() / conversionRate;
  }
  else
  {
    int64 conversionRate = timeScale.TicksPerSecond() / time_duration::ticks_per_second();
    this->ticks_ += timePart.ticks() * conversionRate;
  }
}

ptime Time::ToPosixTime() const
{
  return this->ToPosixTime(TimeScale::Master());
}

ptime Time::ToPosixTime(TimeScale& timeScale) const
{
  auto ticks = this->ticks_;

  //  adjust ticks for epoch 1970
  int64 epochDelta = Epoch1970 - timeScale.Epoch();
  ticks -= epochDelta * timeScale.TicksPerDay(); // now the ticks are epoch'd 1970, so we use 1970,1,1 below

  // create time duration considering resolution.
  if(time_duration::ticks_per_second() > timeScale.TicksPerSecond()) // since the following computations are integer calculcations, we distinguish here
  {
    int64 conversionRate = time_duration::ticks_per_second() / timeScale.TicksPerSecond();
    ticks = conversionRate * ticks;
  }
  else
  {
    int64 conversionRate = timeScale.TicksPerSecond() / time_duration::ticks_per_second();
    ticks = ticks / conversionRate;
  }

  // date(1970) + ptime ticks since (1970)
  time_duration durationSinceEpoch(0, 0, 0, ticks);
  ptime pt(date(1970, 1, 1), durationSinceEpoch);
  return pt;
}

Time Time::AddDays(int numberOfDays)
{
    return Time(this->ticks_ + numberOfDays * TimeScale::Master().TicksPerDay());
}

Time Time::Date() const
{
  return Time(this->ticks_ - this->ticks_ % TimeScale::Master().TicksPerDay());
}

int Time::Year() const
{
  return this->ToPosixTime().date().year();
}

int Time::Month() const
{
  return this->ToPosixTime().date().month();
}

int Time::Day() const
{
  return this->ToPosixTime().date().day();
}

time_duration Time::TimeOfDay() const
{
  return this->ToPosixTime().time_of_day();
}

int Time::Hour() const
{
  return this->TimeOfDay().hours();
}

int Time::Minute() const
{
  return this->TimeOfDay().minutes();
}

int Time::Second() const
{
  return this->TimeOfDay().seconds();
}

bool Time::operator> (const Time& other) const
{
  return this->ticks_ > other.ticks_;
}

bool Time::operator>=(const Time& other) const
{
  return this->ticks_ >= other.ticks_;
}

bool Time::operator< (const Time& other) const
{
  return this->ticks_ < other.ticks_;
}

bool Time::operator<=(const Time& other) const
{
  return this->ticks_ <= other.ticks_;
}

bool Time::operator== (const Time& other) const
{
  return this->ticks_ == other.ticks_;
}

bool Time::operator!=(const Time& other) const
{
  return this->ticks_ != other.ticks_;
}

} // namespace teatime


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

  @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_FUNCTION_DEFINITION_SATURATE_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_DEFINITION_SATURATE_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/dispatch.hpp>
#include <boost/simd/as.hpp>
#include <boost/simd/detail/dispatch/function/make_callable.hpp>
#include <boost/simd/detail/dispatch/hierarchy/functions.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd
{
  namespace tag
  {
    BOOST_DISPATCH_MAKE_TAG(ext, saturate_, boost::dispatch::elementwise_<saturate_>);
  }

  namespace ext
  {
    BOOST_DISPATCH_FUNCTION_DECLARATION(tag, saturate_);
  }

  namespace functional
  {
    BOOST_DISPATCH_CALLABLE_DEFINITION(tag::saturate_,saturate);
  }

  template<typename T, typename A> BOOST_FORCEINLINE A saturate(const A& a) BOOST_NOEXCEPT
  {
    return functional::saturate(a, as_<T>());
  }
} }

#endif