// from ros-control meta packages
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>

#include <pluginlib/class_list_macros.h>
#include <std_msgs/Float64MultiArray.h>

#include <urdf/model.h>

#include <Eigen/LU>
// from kdl packages
#include <kdl/tree.hpp>
#include <kdl/kdl.hpp>
#include <kdl/chain.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chaindynparam.hpp>              // inverse dynamics
#include <kdl/jntarrayacc.hpp>
#include <kdl/jntarray.hpp>
#include <math.h>
#include <numeric>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainfksolvervel_recursive.hpp>
#include <kdl/framevel.hpp>

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

#define PI 3.141592
#define D2R PI / 180.0
#define R2D 180.0 / PI
#define SaveDataMax 49

namespace arm_controllers
{
class GravityControllerReactive : 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_);
        seg_jac_1.resize(1);
        seg_jac_2.resize(2);
        seg_jac_3.resize(3);
        seg_jac_4.resize(4);
        seg_jac_5.resize(5);
        J_.resize(n_joints_);
        K_kine_.resize(n_joints_);
        K_kine_.data(0) = 1.0;
        K_kine_.data(1) = 1.0;
        K_kine_.data(2) = 1.0;
        K_kine_.data(3) = 1.0;
        K_kine_.data(4) = 1.0;
        K_kine_.data(5) = 1.0;

        std::vector<double> Kp(n_joints_), Ki(n_joints_), Kd(n_joints_);
        for (size_t i = 0; i < n_joints_; i++)
        {
            std::string si = boost::lexical_cast<std::string>(i + 1);
            if (n.getParam("/elfin/gravity_controller_reactive/gains/elfin_joint" + si + "/pid/p", Kp[i]))
            {
                Kp_(i) = Kp[i];
            }
            else
            {
                std::cout << "/elfin/gravity_controller_reactive/gains/elfin_joint" + si + "/pid/p" << std::endl;
                ROS_ERROR("Cannot find pid/p gain");
                return false;
            }

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

            if (n.getParam("/elfin/gravity_controller_reactive/gains/elfin_joint" + si + "/pid/d", Kd[i]))
            {
                Kd_(i) = Kd[i];
            }
            else
            {
                ROS_ERROR("Cannot find pid/d 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_name;
        if (!n.getParam("root_link", root_name))
        {
            ROS_ERROR("Could not find root link name");
            return false;
        }
        if (!n.getParam("tip_link", tip_name))
        {
            ROS_ERROR("Could not find tip link name");
            return false;
        }
        if (!kdl_tree_.getChain(root_name, tip_name, kdl_chain_))
        {
            ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
            ROS_ERROR_STREAM("  " << root_name << " --> " << tip_name);
            ROS_ERROR_STREAM("  Tree has " << kdl_tree_.getNrOfJoints() << " joints");
            ROS_ERROR_STREAM("  Tree has " << kdl_tree_.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("Got kdl chain");
        }

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

        id_solver_.reset(new KDL::ChainDynParam(kdl_chain_, gravity_));
        FKSolver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
        FKSolver_vel_.reset(new KDL::ChainFkSolverVel_recursive(kdl_chain_));
        jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));

        kdl_tree_.getChain("world", "elfin_link1", kdl_chain_1);
        jac_solver_1.reset(new KDL::ChainJntToJacSolver(kdl_chain_1));
        fk_solver_1.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_1));

        kdl_tree_.getChain("world", "elfin_link2", kdl_chain_2);
        jac_solver_2.reset(new KDL::ChainJntToJacSolver(kdl_chain_2));
        fk_solver_2.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_2));

        kdl_tree_.getChain("world", "elfin_link3", kdl_chain_3);
        jac_solver_3.reset(new KDL::ChainJntToJacSolver(kdl_chain_3));
        fk_solver_3.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_3));

        kdl_tree_.getChain("world", "elfin_link4", kdl_chain_4);
        jac_solver_4.reset(new KDL::ChainJntToJacSolver(kdl_chain_4));
        fk_solver_4.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_4));

        kdl_tree_.getChain("world", "elfin_link5", kdl_chain_5);
        jac_solver_5.reset(new KDL::ChainJntToJacSolver(kdl_chain_5));
        fk_solver_5.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_5));

        // ********* 5. 각종 변수 초기화 *********

        // 5.1 Vector 초기화 (사이즈 정의 및 값 0)
        tau_d_.data = Eigen::VectorXd::Zero(n_joints_);

        qd_.data = Eigen::VectorXd::Zero(n_joints_);
        qd_dot_.data = Eigen::VectorXd::Zero(n_joints_);
        qd_ddot_.data = Eigen::VectorXd::Zero(n_joints_);
        qd_old_.data = Eigen::VectorXd::Zero(n_joints_);

        q_.data = Eigen::VectorXd::Zero(n_joints_);
        qdot_.data = Eigen::VectorXd::Zero(n_joints_);
        xdot_.data = Eigen::VectorXd::Zero(n_joints_);
        ex_ = Eigen::VectorXd::Zero(n_joints_);
        jnt_limits_lower_.data = Eigen::VectorXd::Zero(n_joints_);
        jnt_limits_upper_.data = Eigen::VectorXd::Zero(n_joints_);
        diff_to_low_.data = Eigen::VectorXd::Zero(n_joints_);
        diff_to_low_prev_.data = Eigen::VectorXd::Zero(n_joints_);
        diff_to_upper_.data = Eigen::VectorXd::Zero(n_joints_);
        diff_to_upper_prev_.data = Eigen::VectorXd::Zero(n_joints_);
        ex_obstacle_prev_ = Eigen::VectorXd::Zero(n_joints_);

        obstacle_.p(0) = -0.1;
        obstacle_.p(1) = -0.1;
        obstacle_.p(2) = 0.75;
        obstacle_.M.RPY(0,0,0);

        jnt_limits_lower_(0) = -0.6;
        jnt_limits_lower_(1) = -0.6;
        jnt_limits_lower_(2) = -0.6;
        jnt_limits_lower_(3) = -0.6;
        jnt_limits_lower_(4) = -0.6;
        jnt_limits_lower_(5) = -0.6;

        jnt_limits_upper_(0) = 0.6;
        jnt_limits_upper_(1) = 0.6;
        jnt_limits_upper_(2) = 0.6;
        jnt_limits_upper_(3) = 0.6;
        jnt_limits_upper_(4) = 0.6;
        jnt_limits_upper_(5) = 0.6;

        table_corner_1 = KDL::Vector(-0.2, -0.25, 0.3);
        table_corner_2 = KDL::Vector(-0.9, -0.25, 0.3);
        table_corner_3 = KDL::Vector(-0.9, 0.25, 0.3);
        table_corner_4 = KDL::Vector(-0.2, 0.25, 0.3);

        interpolated_table_surface_.push_back(table_corner_1);
        interpolated_table_surface_.push_back(table_corner_2);
     //   interpolated_table_surface_.push_back(table_corner_3);
     //   interpolated_table_surface_.push_back(table_corner_4);

        int split_num = 30;
        for(int i = 1; i < split_num; i++){
            double diff12 = (table_corner_2(0) - table_corner_1(0)) * i/split_num;
            interpolated_table_surface_.push_back(KDL::Vector(table_corner_1(0) + diff12, -0.25, 0.3));
         //   double diff23 = (table_corner_3(1) - table_corner_2(1)) * i/split_num;
         //   interpolated_table_surface_.push_back(KDL::Vector(-0.9, diff23, 0.5));
         //   double diff34 = (table_corner_4(0) - table_corner_3(0)) * i/split_num;
         //   interpolated_table_surface_.push_back(KDL::Vector(diff34, 0.25, 0.5));
         //   double diff41 = (table_corner_1(1) - table_corner_4(1)) * i/split_num;
         //   interpolated_table_surface_.push_back(KDL::Vector(-0.2, diff41, 0.5));
        }
        std::vector<KDL::Vector> tmp_copy = interpolated_table_surface_;
        for(int i = 1; i < (int)tmp_copy.size(); i++){
            double curr_x = tmp_copy[i](0);
            for(int j = 0; j <= split_num; j++){
                double next_y = tmp_copy[i](1) + 0.5*j/split_num;
                interpolated_table_surface_.push_back(KDL::Vector(curr_x, next_y, 0.3));
            }
        }
        std::cout << "TABLE HAS: " << interpolated_table_surface_.size() << std::endl;

        for(int i = 1; i <= 5; i++){
            KDL::JntArray arr;
            arr.data = Eigen::VectorXd::Zero(i);
            arm_segment_states_.push_back(arr);
        }
        std::cout << "JntArrays done " << std::endl;

        e_.data = Eigen::VectorXd::Zero(n_joints_);
        e_dot_.data = Eigen::VectorXd::Zero(n_joints_);
        e_int_.data = Eigen::VectorXd::Zero(n_joints_);

        // 5.2 Matrix 초기화 (사이즈 정의 및 값 0)
        M_.resize(kdl_chain_.getNrOfJoints());
        C_.resize(kdl_chain_.getNrOfJoints());
        G_.resize(kdl_chain_.getNrOfJoints());

        // ********* 6. ROS 명령어 *********
        // 6.1 publisher
        pub_qd_ = n.advertise<std_msgs::Float64MultiArray>("qd", 1000);
        pub_q_ = n.advertise<std_msgs::Float64MultiArray>("q", 1000);
        pub_e_ = n.advertise<std_msgs::Float64MultiArray>("e", 1000);

        pub_SaveData_ = n.advertise<std_msgs::Float64MultiArray>("SaveData", 1000); // 뒤에 숫자는?

        // 6.2 subsriber

        return true;
    }

    void commandCB(const std_msgs::Float64MultiArrayConstPtr &msg)
    {
        if (msg->data.size() != n_joints_)
        {
            ROS_ERROR_STREAM("Dimension of command (" << msg->data.size() << ") does not match number of joints (" << n_joints_ << ")! Not executing!");
            return;
        }
    }

    void starting(const ros::Time &time)
    {
        t = 0.0;
        ROS_INFO("Starting Reactive Controller");
    }

    void update(const ros::Time &time, const ros::Duration &period)
    {
        // ********* 0. Get states from gazebo *********
        // 0.1 sampling time
        double dt = period.toSec();
        t = t + 0.001;

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

        int segment_length = 1;
        for(int i = 0; i < 5; i++){
            for(int j = 1; j <= segment_length; j++){
                arm_segment_states_[i](j) = q_(j);
            }
            segment_length++;
        }

        std::vector<KDL::Jacobian> jacobians_;
        jac_solver_1->JntToJac(arm_segment_states_[0], seg_jac_1);
        jacobians_.push_back(seg_jac_1);

        jac_solver_2->JntToJac(arm_segment_states_[1], seg_jac_2);
        jacobians_.push_back(seg_jac_2);

        jac_solver_3->JntToJac(arm_segment_states_[2], seg_jac_3);
        jacobians_.push_back(seg_jac_3);

        jac_solver_4->JntToJac(arm_segment_states_[3], seg_jac_4);
        jacobians_.push_back(seg_jac_4);

        jac_solver_5->JntToJac(arm_segment_states_[4], seg_jac_5);
        jacobians_.push_back(seg_jac_5);

        // ********* 1. Desired Trajecoty in Joint Space *********

        for (size_t i = 0; i < n_joints_; i++)
        {
            qd_ddot_(i) = -M_PI * M_PI / 4 * 45 * KDL::deg2rad * sin(M_PI / 2 * t);
            qd_dot_(i) = M_PI / 2 * 45 * KDL::deg2rad * cos(M_PI / 2 * t);
            qd_(i) = 45 * KDL::deg2rad * sin(M_PI / 2* t);
          //  continue;
         //
            if( t < 1 ){
                qd_(i) = 0;
            }
           // qd_(i) = 0;
            qd_ddot_(i) = 0;
            qd_dot_(i) = 0;

            // This is the sequence to bring the arm to starting position.
            if(t < 2){
               // qd_(i) = 0;
                qd_(0) = -1.6;
                qd_(1) = 0;
                qd_(2) = -1.4;
                qd_(3) = 0.01;
                qd_(4) = -1.57;
                qd_(5) = 0;
            }

            // This is the sweeping sequence.
            if(t > 2){
          //     qd_(i) = 1.57;
                //qd_(0) = q_(0) + dt*30;
           //     qd_(0) += dt;
             //   qd_(1) = 0;
               // qd_(2) = -1.4;
            //    qd_(3) = 0.01;
              //  qd_(4) = -1.57;
                //qd_(5) = 0;
              //  qd_(1) = 35 * KDL::deg2rad * sin(M_PI / 2* t);
            }
            if(t < 1){
               // qd_(i) = 0;
                qd_(0) = 0;
                qd_(1) = 0;
                qd_(2) = 0;
                qd_(3) = 0.0;
                qd_(4) = 0;
                qd_(5) = 0;
            }

            // This is the sweeping sequence.
            if(t > 1){
          //     qd_(i) = 1.57;
                //qd_(0) = q_(0) + dt*30;
                qd_(0) = 0;
                //qd_(1) = 0;
                //qd_(2) = 0;
                qd_(1) = -55 * KDL::deg2rad * sin(M_PI * t);
                qd_(2) = -55 * KDL::deg2rad * sin(M_PI * t);
                qd_(3) = 0;
                qd_(4) = 0;
                qd_(5) = 0;

            }

        }

        tau_rep_ = Eigen::VectorXd::Zero(n_joints_);

        // *** 2.2.1 Compute model(M,C,G) ***
        id_solver_->JntToMass(q_, M_);
        id_solver_->JntToCoriolis(q_, qdot_, C_);
        id_solver_->JntToGravity(q_, G_);

        // Apply Joint limits as repulsive forces/torques
        KDL::JntArray diff_q_;
        Subtract(q_, qd_, diff_q_);
        Subtract(q_, jnt_limits_lower_, diff_to_low_);
        Subtract(q_, jnt_limits_upper_, diff_to_upper_);

 //       Eigen::Matrix<double, 6, 1> U_att = (1/2) * diff_q_.data.cwiseProduct(diff_q_.data);
 //       Eigen::Matrix<double, 6, 1> U_rep_low_ = -(1/2) * diff_to_low_.data.cwiseProduct(diff_to_low_.data);
 //       Eigen::Matrix<double, 6, 1> U_rep_upper_ = -(1/2) * diff_to_upper_.data.cwiseProduct(diff_to_upper_.data);

        KDL::Frame xd_;
        KDL::Frame x_up_lim_;
        KDL::Frame x_low_lim_;
        Eigen::Matrix<double, 6, 1> ex_low_;
        Eigen::Matrix<double, 6, 1> ex_upper_;
        Eigen::Matrix<double, 6, 1> ex_obstacle_;

        jnt_to_jac_solver_->JntToJac(q_, J_);
        FKSolver_->JntToCart(qd_, xd_);
        FKSolver_->JntToCart(jnt_limits_lower_, x_low_lim_);
        FKSolver_->JntToCart(jnt_limits_upper_, x_up_lim_);
        FKSolver_->JntToCart(q_, x_);

        // Calculate workspace positions for joints and middle parts of links.
        std::vector<KDL::Frame> arm_segment_states_cart_;
        std::vector<KDL::Frame> arm_segment_midpoints_states_cart_;

        KDL::Frame seg_cart_;
        fk_solver_1->JntToCart(arm_segment_states_[0], seg_cart_);
        arm_segment_states_cart_.push_back(seg_cart_);
        fk_solver_2->JntToCart(arm_segment_states_[1], seg_cart_);
        arm_segment_states_cart_.push_back(seg_cart_);
        fk_solver_3->JntToCart(arm_segment_states_[2], seg_cart_);
        arm_segment_states_cart_.push_back(seg_cart_);
        fk_solver_4->JntToCart(arm_segment_states_[3], seg_cart_);
        arm_segment_states_cart_.push_back(seg_cart_);
        fk_solver_5->JntToCart(arm_segment_states_[4], seg_cart_);
        arm_segment_states_cart_.push_back(seg_cart_);

        for(int i = 0; i < (int)arm_segment_states_cart_.size() - 1; i++){
            KDL::Frame start_ = arm_segment_states_cart_[i];
            KDL::Frame end_ = arm_segment_states_cart_[i+1];
            KDL::Frame mid_point_;
            mid_point_.p(0) = (start_.p(0) + end_.p(0)) / 2;
            mid_point_.p(1) = (start_.p(1) + end_.p(1)) / 2;
            mid_point_.p(2) = (start_.p(2) + end_.p(2)) / 2;
            mid_point_.M.RPY(0,0,0);
            arm_segment_midpoints_states_cart_.push_back(mid_point_);
        }

        ex_temp_ = diff(x_, xd_);

        ex_(0) = ex_temp_(0);
        ex_(1) = ex_temp_(1);
        ex_(2) = ex_temp_(2);
        ex_(3) = ex_temp_(3);
        ex_(4) = ex_temp_(4);
        ex_(5) = ex_temp_(5);

        ex_temp_ = diff(x_, x_up_lim_);

        ex_upper_(0) = ex_temp_(0);
        ex_upper_(1) = ex_temp_(1);
        ex_upper_(2) = ex_temp_(2);
        ex_upper_(3) = ex_temp_(3);
        ex_upper_(4) = ex_temp_(4);
        ex_upper_(5) = ex_temp_(5);

        ex_temp_ = diff(x_, x_low_lim_);

        ex_low_(0) = ex_temp_(0);
        ex_low_(1) = ex_temp_(1);
        ex_low_(2) = ex_temp_(2);
        ex_low_(3) = ex_temp_(3);
        ex_low_(4) = ex_temp_(4);
        ex_low_(5) = ex_temp_(5);

        ex_obstacle_ = ex_obstacle_prev_;

        ex_temp_ = diff(obstacle_, x_);

        ex_obstacle_(0) = ex_temp_(0);
        ex_obstacle_(1) = ex_temp_(1);
        ex_obstacle_(2) = ex_temp_(2);
        ex_obstacle_(3) = ex_temp_(3);
        ex_obstacle_(4) = ex_temp_(4);
        ex_obstacle_(5) = ex_temp_(5);

        Eigen::Matrix<double, 6, 1> grad_D_ = ex_obstacle_ - ex_obstacle_prev_;

     //   Eigen::Matrix<double, 6, 1> U_att = (1/2) * ex_.cwiseProduct(ex_);
        Eigen::Matrix<double, 6, 1> U_rep_lower_ = -(1/2) * ex_low_.cwiseProduct(ex_low_);
        Eigen::Matrix<double, 6, 1> U_rep_upper_ = -(1/2) * ex_upper_.cwiseProduct(ex_upper_);
        Eigen::Matrix<double, 6, 1> U_rep_ = U_rep_lower_ + U_rep_upper_;

       // Eigen::Matrix<double, 6, 1> F_att = -ex_;
        Eigen::Matrix<double, 6, 1> F_rep_lower_; // = (1 / ex_low_) * (1 / ex_low_.cwiseProduct(ex_low_));
        Eigen::Matrix<double, 6, 1> F_rep_upper_; // = (1 / ex_upper_) * (1 / ex_upper_.cwiseProduct(ex_upper_));
        Eigen::Matrix<double, 6, 1> F_obstacle_;

        // This is where the joint limits are applied.
        double Q_star_ = 0.0;

        Eigen::Matrix<double, 6, 1> grad_D_low_ = diff_to_low_.data - diff_to_low_prev_.data;
        Eigen::Matrix<double, 6, 1> grad_D_upper_ = diff_to_upper_.data - diff_to_upper_prev_.data;
        for(int i = 0; i < 6; i++){

            // UNCOMMENT THIS break to include joint limit forces.
             break;
            if( abs(diff_to_low_(i)) < Q_star_ ){
                tau_rep_(i) += 0.001*(1 / diff_to_low_(i) - 1 / Q_star_) * (1 / diff_to_low_(i) - 1 / Q_star_);
            }
            if( abs(diff_to_upper_(i)) < Q_star_){
                tau_rep_(i) += -0.001 * (1 / diff_to_upper_(i) - 1 / Q_star_) * (1 / diff_to_upper_(i) - 1 / Q_star_);
            }
        }
        diff_to_low_prev_ = diff_to_low_;
        diff_to_upper_prev_ = diff_to_upper_;

        double Q_star_table_ = 0.22;
        double closest_dist_ = 999;
        int closest_idx_ = -2;
        Eigen::Matrix<double, 6, 1> ex_table_point_ = Eigen::VectorXd::Zero(6);
        Eigen::Matrix<double, 6, 1> ex_closest_table_point_ = Eigen::VectorXd::Zero(6);
        Eigen::Matrix<double, 6, 1> F_rep_table_total_ = Eigen::VectorXd::Zero(6);

        // Include repulsion field of the table WRT end effector only.
        for(int i = 0; i < (int)interpolated_table_surface_.size(); i++){
           // break;
            KDL::Vector curr_table_point_ = interpolated_table_surface_[i];
            ex_table_point_(0) = curr_table_point_(0) - x_.p(0);
            ex_table_point_(1) = curr_table_point_(1) - x_.p(1);
            ex_table_point_(2) = curr_table_point_(2) - x_.p(2);
            table_point_distances_[i] = -ex_table_point_;

            double dist = sqrt( ex_table_point_(0)*ex_table_point_(0) + ex_table_point_(1)*ex_table_point_(1) + ex_table_point_(2)*ex_table_point_(2));
            if( dist < Q_star_table_ ){
                closest_dist_ = dist;
                closest_idx_ = i;
                ex_closest_table_point_ = -ex_table_point_;

                Eigen::Matrix<double, 6, 1> grad_D_table_ = ex_table_point_ - table_point_distances_prev_[i];

                F_rep_table_total_(0) = -0.1 * (1 / ex_table_point_(0) - 1 / Q_star_table_) * (1 / (ex_table_point_(0) * ex_table_point_(0)) * -grad_D_table_(0));
                F_rep_table_total_(1) = -0.1 * (1 / ex_table_point_(1) - 1 / Q_star_table_) * (1 / (ex_table_point_(1) * ex_table_point_(1)) * -grad_D_table_(1));
                F_rep_table_total_(2) = -10 * (1 / ex_table_point_(2) - 1 / Q_star_table_) * (1 / (ex_table_point_(2) * ex_table_point_(2)) * -grad_D_table_(2));
                for(int f = 0; f < 3; f++){
                    if( F_rep_table_total_(f) > 500 ){
                        F_rep_table_total_(f) = 500;
                    }
                }

                // UNCOMMENT THIS to include the tables repulsion forces in the arm.
               tau_rep_ += J_.data.transpose() * F_rep_table_total_;
            }
        }

        // Include repulsion field of the table WRT all physical points of arm.
        for(int i = 0; i < (int)interpolated_table_surface_.size(); i++){
          // break;
            KDL::Vector curr_table_point_ = interpolated_table_surface_[i];

            for(int a = 1; a < (int)arm_segment_states_cart_.size(); a++ ){
                KDL::Frame arm_point_ = arm_segment_states_cart_[a];
                ex_table_point_(0) = curr_table_point_(0) - arm_point_.p(0);
                ex_table_point_(1) = curr_table_point_(1) - arm_point_.p(1);
                ex_table_point_(2) = curr_table_point_(2) - arm_point_.p(2);
                table_point_distances_arm_[a][i] = -ex_table_point_;

                double dist = sqrt( ex_table_point_(0)*ex_table_point_(0) + ex_table_point_(1)*ex_table_point_(1) + ex_table_point_(2)*ex_table_point_(2));
                if( dist < Q_star_table_ ){

                    closest_dist_ = dist;
                    closest_idx_ = i;
                    ex_closest_table_point_ = -ex_table_point_;

                    std::cout << "ROWS: " << jacobians_[a].rows()  << " COLUMNS: " << jacobians_[a].columns() << std::endl;
                    int c_size = jacobians_[a].columns();
                    //Eigen::VectorXd F_rep_table_total_segs_ = Eigen::VectorXd::Zero(jacobians_[a].columns());
                    Eigen::Matrix<double, 6, 1> grad_D_table_ = ex_table_point_ - table_point_distances_arm_prev_[a][i];

                   // F_rep_table_total_(0) = -0.01 * (1 / ex_table_point_(0) - 1 / Q_star_table_) * (1 / (ex_table_point_(0) * ex_table_point_(0)) * -grad_D_table_(0));
                   // F_rep_table_total_(1) = -0.01 * (1 / ex_table_point_(1) - 1 / Q_star_table_) * (1 / (ex_table_point_(1) * ex_table_point_(1)) * -grad_D_table_(1));
                    F_rep_table_total_(2) = -1 * (1 / ex_table_point_(2) - 1 / Q_star_table_) * (1 / (ex_table_point_(2) * ex_table_point_(2)) * -grad_D_table_(2));
                    for(int f = 0; f < 3; f++){
                        if( F_rep_table_total_(f) > 1000 ){
                            F_rep_table_total_(f) = 1000;
                        }
                    }
                   std::cout << "x: " << F_rep_table_total_(0) << " y: " << F_rep_table_total_(1) << " z: "  << F_rep_table_total_(2) << std::endl;
                // UNCOMMENT THIS to include the tables repulsion forces in the arm.
                   Eigen::VectorXd torques = jacobians_[a].data.transpose() * F_rep_table_total_;
                   for(int t = 0; t < c_size; t++){
                      tau_rep_(t) += torques(t) - (M_.data * qdot_.data)(t);
                   }
                }
            }
        }
        tau_rep_ -= M_.data * qdot_.data;

       // ex_table_prev_ = ex_closest_table_point_;
        table_point_distances_prev_ = table_point_distances_;
        table_point_distances_arm_prev_ = table_point_distances_arm_;

        //Eigen::Matrix<double, 6, 1> F_rep_ = F_obstacle_;
        Eigen::Matrix<double, 6, 1> F_rep_ = F_rep_table_total_;
        F_rep_(3) = 0;
        F_rep_(4) = 0;
        F_rep_(5) = 0;

        // ********* 2. Motion Controller in Joint Space*********
        // *** 2.1 Error Definition in Joint Space ***
        e_.data = qd_.data - q_.data;
        e_dot_.data = qd_dot_.data - qdot_.data;
        e_int_.data = qd_.data - q_.data; // (To do: e_int 업데이트 필요요)

        // *** 2.2.2 gravity + PD as in slides ***

        // This can be considered as u_att
        tau_PD_grav_.data = G_.data + Kp_.data.cwiseProduct(e_.data) - Kd_.data.cwiseProduct(qdot_.data);

        //tau_rep_ = J_.data.transpose() * F_rep_ - M_.data * qdot_.data;
       // tau_rep_ = J_.data.transpose() * F_rep_ - Kd_.data.cwiseProduct(qdot_.data);
        KDL::JntArray tau_total_;
        tau_total_.data = tau_PD_grav_.data + tau_rep_;

        // *** 2.3 Apply Torque Command to Actuator ***

        // Use 'e_dot_.data = qdot_c_.data - qdot_.data' for kinematic controller.
        // Otherwise use 'e_dot_.data = qd_dot_.data - qdot_.data'.
        //e_dot_.data = qdot_c_.data - qdot_.data;
        //aux_d_.data = M_.data * (qd_ddot_.data + Kd_.data.cwiseProduct(e_dot_.data));
        //comp_d_.data = C_.data + G_.data;
        //tau_d_.data = aux_d_.data + comp_d_.data;

        for (int i = 0; i < n_joints_; i++)
        {
            joints_[i].setCommand(tau_total_(i));
            //joints_[i].setCommand(tau_PD_grav_(i));
            //joints_[i].setCommand(tau_d_(i));
        }

        // ********* 3. data 저장 *********
        save_data();

        // ********* 4. state 출력 *********
        print_state();
    }

    void stopping(const ros::Time &time)
    {
    }

    void save_data()
    {
        // 1
        // Simulation time (unit: sec)
        SaveData_[0] = t;

        // Desired position in joint space (unit: rad)
        SaveData_[1] = qd_(0);
        SaveData_[2] = qd_(1);
        SaveData_[3] = qd_(2);
        SaveData_[4] = qd_(3);
        SaveData_[5] = qd_(4);
        SaveData_[6] = qd_(5);

        // Desired velocity in joint space (unit: rad/s)
        SaveData_[7] = qd_dot_(0);
        SaveData_[8] = qd_dot_(1);
        SaveData_[9] = qd_dot_(2);
        SaveData_[10] = qd_dot_(3);
        SaveData_[11] = qd_dot_(4);
        SaveData_[12] = qd_dot_(5);

        // Desired acceleration in joint space (unit: rad/s^2)
        SaveData_[13] = qd_ddot_(0);
        SaveData_[14] = qd_ddot_(1);
        SaveData_[15] = qd_ddot_(2);
        SaveData_[16] = qd_ddot_(3);
        SaveData_[17] = qd_ddot_(4);
        SaveData_[18] = qd_ddot_(5);

        // Actual position in joint space (unit: rad)
        SaveData_[19] = q_(0);
        SaveData_[20] = q_(1);
        SaveData_[21] = q_(2);
        SaveData_[22] = q_(3);
        SaveData_[23] = q_(4);
        SaveData_[24] = q_(5);

        // Actual velocity in joint space (unit: rad/s)
        SaveData_[25] = qdot_(0);
        SaveData_[26] = qdot_(1);
        SaveData_[27] = qdot_(2);
        SaveData_[28] = qdot_(3);
        SaveData_[29] = qdot_(4);
        SaveData_[30] = qdot_(5);

        // Error position in joint space (unit: rad)
        SaveData_[31] = e_(0);
        SaveData_[32] = e_(1);
        SaveData_[33] = e_(2);
        SaveData_[34] = e_(3);
        SaveData_[35] = e_(4);
        SaveData_[36] = e_(5);

        j1_avg.push_back(abs(R2D * e_(0)));
        j2_avg.push_back(abs(R2D * e_(1)));
        j3_avg.push_back(abs(R2D * e_(2)));
        j4_avg.push_back(abs(R2D * e_(3)));
        j5_avg.push_back(abs(R2D * e_(4)));
        j6_avg.push_back(abs(R2D * e_(5)));

        // Error velocity in joint space (unit: rad/s)
        SaveData_[37] = e_dot_(0);
        SaveData_[38] = e_dot_(1);
        SaveData_[39] = e_dot_(3);
        SaveData_[40] = e_dot_(4);
        SaveData_[41] = e_dot_(5);
        SaveData_[42] = e_dot_(6);

        j1_vel_avg.push_back(abs(R2D * e_dot_(0)));
        j2_vel_avg.push_back(abs(R2D * e_dot_(1)));
        j3_vel_avg.push_back(abs(R2D * e_dot_(2)));
        j4_vel_avg.push_back(abs(R2D * e_dot_(3)));
        j5_vel_avg.push_back(abs(R2D * e_dot_(4)));
        j6_vel_avg.push_back(abs(R2D * e_dot_(5)));

        // Error intergal value in joint space (unit: rad*sec)
        SaveData_[43] = e_int_(0);
        SaveData_[44] = e_int_(1);
        SaveData_[45] = e_int_(2);
        SaveData_[46] = e_int_(3);
        SaveData_[47] = e_int_(4);
        SaveData_[48] = e_int_(5);

        // 2
        msg_qd_.data.clear();
        msg_q_.data.clear();
        msg_e_.data.clear();

        msg_SaveData_.data.clear();

        // 3
        for (int i = 0; i < n_joints_; i++)
        {
            msg_qd_.data.push_back(qd_(i));
            msg_q_.data.push_back(q_(i));
            msg_e_.data.push_back(e_(i));
        }

        for (int i = 0; i < SaveDataMax; i++)
        {
            msg_SaveData_.data.push_back(SaveData_[i]);
        }

        // 4
        pub_qd_.publish(msg_qd_);
        pub_q_.publish(msg_q_);
        pub_e_.publish(msg_e_);

        pub_SaveData_.publish(msg_SaveData_);
    }

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

            printf("*** Desired State in Joint Space (unit: deg) ***\n");
            printf("qd_(0): %f, ", qd_(0)*R2D);
            printf("qd_(1): %f, ", qd_(1)*R2D);
            printf("qd_(2): %f, ", qd_(2)*R2D);
            printf("qd_(3): %f, ", qd_(3)*R2D);
            printf("qd_(4): %f, ", qd_(4)*R2D);
            printf("qd_(5): %f\n", qd_(5)*R2D);
            printf("\n");

            printf("*** Actual State in Joint Space (unit: deg) ***\n");
            printf("q_(0): %f, ", q_(0) * R2D);
            printf("q_(1): %f, ", q_(1) * R2D);
            printf("q_(2): %f, ", q_(2) * R2D);
            printf("q_(3): %f, ", q_(3) * R2D);
            printf("q_(4): %f, ", q_(4) * R2D);
            printf("q_(5): %f\n", q_(5) * R2D);
            printf("\n");

            printf("*** Joint Space Vel Error (unit: deg)  ***\n");
            printf("%f, ", R2D * e_dot_(0));
            printf("%f, ", R2D * e_dot_(1));
            printf("%f, ", R2D * e_dot_(2));
            printf("%f, ", R2D * e_dot_(3));
            printf("%f, ", R2D * e_dot_(4));
            printf("%f\n", R2D * e_dot_(5));
            printf("\n");

            int num_vals = j1_vel_avg.size();
            double average = 0.0;
            if ( num_vals != 0) {
                average = std::accumulate( j1_vel_avg.begin(), j1_vel_avg.end(), 0.0) / num_vals;
                std::cout << "JOINT VEL ERROR AVERAGES: " << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j2_vel_avg.begin(), j2_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j3_vel_avg.begin(), j3_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j4_vel_avg.begin(), j4_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j5_vel_avg.begin(), j5_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << " - ";
                average = 0.0;
                average = std::accumulate( j6_vel_avg.begin(), j6_vel_avg.end(), 0.0) / num_vals;
                std::cout << std::to_string(average) << std::endl;
            }

            count = 0;
        }
        count++;
    }

  private:
    // others
    double t;

    //Joint handles
    unsigned int n_joints_;                               // joint 숫자
    std::vector<std::string> joint_names_;                // joint name ??
    std::vector<hardware_interface::JointHandle> joints_; // ??
    std::vector<urdf::JointConstSharedPtr> joint_urdfs_;  // ??

    // kdl
    KDL::Tree kdl_tree_;   // tree?
    KDL::Chain kdl_chain_; // chain?

    // kdl M,C,G
    KDL::JntSpaceInertiaMatrix M_; // intertia matrix
    KDL::JntSpaceInertiaMatrix M_inv_;
    KDL::JntArray C_;              // coriolis
    KDL::JntArray G_;              // gravity torque vector
    KDL::Vector gravity_;

    // kdl solver
    boost::scoped_ptr<KDL::ChainDynParam> id_solver_;                  // Solver To compute the inverse dynamics

    // Joint Space State
    KDL::JntArray qd_, qd_dot_, qd_ddot_;
    KDL::JntArray qd_old_;
    KDL::JntArray q_, qdot_, qddot_;
    KDL::JntArray e_, e_dot_, e_int_, e_ddot_;
    KDL::Jacobian J_;
    KDL::Jacobian seg_jac_1;
    KDL::Jacobian seg_jac_2;
    KDL::Jacobian seg_jac_3;
    KDL::Jacobian seg_jac_4;
    KDL::Jacobian seg_jac_5;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    FKSolver_;
    boost::scoped_ptr<KDL::ChainFkSolverVel>  FKSolver_vel_;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_1;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_2;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_3;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_4;
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jac_solver_5;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_1;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_2;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_3;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_4;
    boost::scoped_ptr<KDL::ChainFkSolverPos>    fk_solver_5;
    KDL::JntArray  xdot_;

    // Input
    KDL::JntArray aux_d_;
    KDL::JntArray aux_d_Kpzero_;
    KDL::JntArray comp_d_;
    KDL::JntArray tau_d_;
    KDL::JntArray tau_PD_grav_;
    KDL::JntArray vel_control_;
    KDL::JntArray kine_control_;
    KDL::JntArray qdot_c_;
    double rotX_pos, rotY_pos, rotZ_pos, rotW_pos, rotX_des, rotY_des, rotZ_des, rotW_des;

    // Joint limits
    KDL::JntArray jnt_limits_lower_;
    KDL::JntArray jnt_limits_upper_;
    KDL::JntArray diff_to_low_;
    KDL::JntArray diff_to_upper_;
    KDL::JntArray diff_to_low_prev_;
    KDL::JntArray diff_to_upper_prev_;
    KDL::Frame obstacle_;
    std::vector<KDL::JntArray> arm_segment_states_;

    Eigen::Matrix<double, 6, 1> ex_obstacle_prev_;
    Eigen::Matrix<double, 3, 1> ex_table_prev_;
    KDL::Vector table_corner_1;
    KDL::Vector table_corner_2;
    KDL::Vector table_corner_3;
    KDL::Vector table_corner_4;
    std::vector<KDL::Vector> interpolated_table_surface_;
    std::map<int, Eigen::Matrix<double, 6, 1>> table_point_distances_;
    std::map<int, Eigen::Matrix<double, 6, 1>> table_point_distances_prev_;
    std::map<int, std::map<int, Eigen::Matrix<double, 6, 1>>> table_point_distances_arm_;
    std::map<int, std::map<int, Eigen::Matrix<double, 6, 1>>> table_point_distances_arm_prev_;
    Eigen::Matrix<double ,6, 1> tau_rep_;
    std::vector<boost::scoped_ptr<KDL::ChainJntToJacSolver>> jac_seg_solvers_;

    // gains
    KDL::JntArray Kp_, Ki_, Kd_, K_kine_;
    KDL::Frame fKine_pos_Frame_;
    KDL::Frame fKine_des_Frame_;
    KDL::Twist ex_temp_;
    Eigen::Matrix<double, 6, 1> ex_;
    KDL::Frame x_;
    KDL::FrameVel fKine_vel_Frame_;
    KDL::JntArrayVel qdot_vel_array_;

    KDL::Chain kdl_chain_1;
    KDL::Chain kdl_chain_2;
    KDL::Chain kdl_chain_3;
    KDL::Chain kdl_chain_4;
    KDL::Chain kdl_chain_5;

    // save the data
    double SaveData_[SaveDataMax];
    std::vector<double> j1_avg;
    std::vector<double> j2_avg;
    std::vector<double> j3_avg;
    std::vector<double> j4_avg;
    std::vector<double> j5_avg;
    std::vector<double> j6_avg;

    std::vector<double> j1_vel_avg;
    std::vector<double> j2_vel_avg;
    std::vector<double> j3_vel_avg;
    std::vector<double> j4_vel_avg;
    std::vector<double> j5_vel_avg;
    std::vector<double> j6_vel_avg;

    // ros publisher
    ros::Publisher pub_qd_, pub_q_, pub_e_;
    ros::Publisher pub_SaveData_;

    // ros message
    std_msgs::Float64MultiArray msg_qd_, msg_q_, msg_e_;
    std_msgs::Float64MultiArray msg_SaveData_;
};
}; // namespace arm_controllers
PLUGINLIB_EXPORT_CLASS(arm_controllers::GravityControllerReactive, controller_interface::ControllerBase)


#pragma once

#include <armadillo>

#include "solver/discretizer/discretizer.hpp"

class Pipeline;

/*!
 * \brief Implementation of Discretizer for the enthalpy form of the energy
 * equation.
 */
class EnthalpyDiscretizer : public Discretizer
{
public:
    /*!
     * \brief Constructor. Calls Discretizer constructor with nEquations = 3.
     * \param nGridPoints Number of grid points
     */
    explicit EnthalpyDiscretizer(
            const arma::uword nGridPoints);

    /*!
     * \brief Discretize implementation. Calculates the coefficients of the
     * discretized governing equations, with the enthalpy form of the energy
     * equation.
     *
     * This is just a wrapper around discretizeFromPrimitives().
     *
     * \param dt Time step [s]
     * \param currentState Current pipeline state
     * \param newState New/guess pipeline state
     */
    virtual void discretize(
            const arma::uword dt,
            const Pipeline& currentState,
            const Pipeline& newState) override;

    /*!
     * \brief Calculates the coefficients of the discretized governing
     * equations, with the enthalpy form of the energy equation, and stores
     * the result in Discretizer::m_term_i, Discretizer::m_term_ipp and
     * Discretizer::m_boundaryTerm.
     *
     * \param dt Time step [s]
     * \param diameter Inner diameter [m]
     * \param height Height profile [m]
     * \param gridPoints Grid points [m]
     * \param currentSpecificGasConstant Current specific gas constant \f$R_{\rm specific}\f$ [J/(kg K)]
     * \param currentMassFlow Current mass flow [kg/s]
     * \param currentPressure Current pressure [Pa]
     * \param currentTemperature Temperature [K]
     * \param guessMassFlow New mass flow [kg/s]
     * \param guessPressure New pressure [Pa]
     * \param guessTemperature New temperature [K]
     * \param guessFriction New friction factor [-]
     * \param guessHeatCapacityConstantPressure New heat capacity \f$c_p\f$ [J/(kg K)]
     * \param guessHeatFlux New heat flux [W/m2]
     * \param guessDensity New gas density [kg/m3]
     * \param guessCompressibilityFactor New gas compressibility factor \f$Z\f$ [-]
     * \param guess_dZdT_p New partial derivative \f$\frac{\partial Z}{\partial T}|_p\f$ [-]
     * \param guess_dZdp New partial derivative \f$\frac{\partial Z}{\partial p}|_T\f$ [-]
     */
    void discretizeFromPrimitives(
            const arma::uword dt,

            const arma::vec& diameter,
            const arma::vec& height,
            const arma::vec& gridPoints,

            const arma::vec& currentSpecificGasConstant,
            const arma::vec& currentMassFlow,
            const arma::vec& currentPressure,
            const arma::vec& currentTemperature,

            const arma::vec& guessMassFlow,
            const arma::vec& guessPressure,
            const arma::vec& guessTemperature,
            const arma::vec& guessFriction,
            const arma::vec& guessHeatCapacityConstantPressure,
            const arma::vec& guessHeatFlux,
            const arma::vec& guessDensity,
            const arma::vec& guessCompressibilityFactor,
            const arma::vec& guess_dZdT_p,
            const arma::vec& guess_dZdp);
};


/**
 * \file     definitions.hpp
 * \author   Mehdi Benallegue
 * \date     2013
 * \brief    Definitions of types and some structures.
 *
 * \details
 *
 *
 */

#ifndef STATEOBSERVATIONDEFINITIONSHPP
#define STATEOBSERVATIONDEFINITIONSHPP

//#define STATEOBSERVATION_VERBOUS_CONSTRUCTORS

#include <chrono>
#include <deque>
#include <stdexcept>
#include <vector>

#ifdef STATEOBSERVATION_VERBOUS_CONSTRUCTORS
#  include <iostream>
#endif

#include <boost/assert.hpp>
#include <boost/timer/timer.hpp>

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

#ifndef M_PI
#  include <boost/math/constants/constants.hpp>
#  define M_PI boost::math::constants::pi<double>()
#endif

#include <state-observation/api.h>

// basic file operations
#include <fstream>

namespace stateObservation
{
/// Dynamic sized scalar vector
typedef Eigen::VectorXd Vector;

/// 1D Vector
typedef Eigen::Matrix<double, 1, 1> Vector1;

/// 2d Vector
typedef Eigen::Matrix<double, 2, 1> Vector2;

/// 3D vector
typedef Eigen::Vector3d Vector3;

/// 3D vector unaligned
typedef Eigen::Matrix<double, 3, 1, Eigen::DontAlign> Vector3Unaligned;

/// 4D vector
typedef Eigen::Vector4d Vector4;

/// 6D vector
typedef Eigen::Matrix<double, 6, 1> Vector6;

/// Dynamic sized Matrix
typedef Eigen::MatrixXd Matrix;

/// 1D scalar Matrix
typedef Eigen::Matrix<double, 1, 1> Matrix1;

/// 2D scalar Matrix
typedef Eigen::Matrix2d Matrix2;

/// 3x3 Scalar Matrix
typedef Eigen::Matrix3d Matrix3;

/// 3x3 Scalar Matrix Unaligned
typedef Eigen::Matrix<double, 3, 3, Eigen::DontAlign> Matrix3Unaligned;

/// 4x4 Scalar Matrix
typedef Eigen::Matrix4d Matrix4;

/// 6x6 Scalar Matrix
typedef Eigen::Matrix<double, 6, 6> Matrix6;

/// 12x12 scalar Matrix
typedef Eigen::Matrix<double, 12, 12> Matrix12;

/// Quaternion
typedef Eigen::Quaterniond Quaternion;

/// Quaternion Unaligned
typedef Eigen::Quaternion<double, Eigen::DontAlign> QuaternionUnaligned;

/// Euler Axis/Angle representation of orientation
typedef Eigen::AngleAxis<double> AngleAxis;

typedef Eigen::Index Index;
typedef long int TimeIndex;
typedef Index TimeSize;

#ifndef NDEBUG
static const bool isDebug = true;
#else
static const bool isDebug = false;
#endif // NDEBUG

/// Debug item default value is just a way to give a default value to debug item
/// this was required by a compilation issue on Visual Studio
template<typename T, const T defaultValue = T()>
class DebugItemDefaultValue
{
public:
  static const T v;
};

template<typename T, const T defaultValue>
const T DebugItemDefaultValue<T, defaultValue>::v = defaultValue;

namespace detail
{
typedef DebugItemDefaultValue<bool, true> defaultTrue;

enum errorType
{
  message,
  exception,
  exceptionAddr
};

template<errorType i = message, int dummy = 0>
class DebugItemDefaultError
{
};

template<int dummy>
class DebugItemDefaultError<message, dummy>
{
public:
  static const char * v;
};

template<int dummy>
class DebugItemDefaultError<exception, dummy>
{
public:
  static const std::runtime_error v;
};

template<int dummy>
class DebugItemDefaultError<exceptionAddr, dummy>
{
public:
  static const std::runtime_error * v;
};

template<int dummy>
const char * DebugItemDefaultError<message, dummy>::v = "The Object is not initialized. \
       If this happened during initialization then run command chckitm_set() \
       to switch it to set. And if the initialization is incomplete, run \
       chckitm_reset() afterwards.";

template<int dummy>
const std::runtime_error DebugItemDefaultError<exception, dummy>::v =
    std::runtime_error(DebugItemDefaultError<message>::v);

template<int dummy>
const std::runtime_error * DebugItemDefaultError<exceptionAddr, dummy>::v = &DebugItemDefaultError<exception>::v;

typedef DebugItemDefaultError<message> defaultErrorMSG;
typedef DebugItemDefaultError<exception> defaultException;
typedef DebugItemDefaultError<exceptionAddr> defaultExceptionAddr;

void STATE_OBSERVATION_DLLAPI defaultSum(const Vector & stateVector, const Vector & tangentVector, Vector & sum);
void STATE_OBSERVATION_DLLAPI defaultDifference(const Vector & stateVector1,
                                                const Vector & stateVector2,
                                                Vector & difference);

} // namespace detail

/// Debug item is an item that exists when the debug variable is true,
/// otherwise it is empty and returns only the default value
template<typename T, typename defaultValue = DebugItemDefaultValue<T>, bool debug = true>
class DebugItem
{
public:
  DebugItem() : b_(defaultValue::v) {}
  explicit DebugItem(const T & v) : b_(v) {}
  inline T & operator=(T v)
  {
    return b_ = v;
  }
  inline operator T() const
  {
    return b_;
  }
  inline T set(const T & v)
  {
    return b_ = v;
  }
  T get() const
  {
    return b_;
  }

private:
  T b_;
};

/// this specialization contains no object
template<typename T, typename defaultValue>
class DebugItem<T, defaultValue, false>
{
public:
  DebugItem() {}
  explicit DebugItem(T) {}
  inline T & operator=(T v)
  {
    /// I am not sure what is the best behaviour, is it to return v ot defaultValue::v?
    return v;
  }
  inline operator T() const
  {
    return defaultValue::v;
  }
  inline T set(const T &)
  {
    return defaultValue::v;
  }
  T get() const
  {
    return defaultValue::v;
  }

private:
  /// no object
};

/// this is simply a structure allowing for automatically verifying that
/// the item has been initialized or not. The chckitm_reset() function allows to
/// set it back to "not initialized" state.
/// -lazy means that the "set" value is true all the time if NDEBUG is defined
/// -alwaysCheck means that the check is always performed and throws exception
/// if it fails. Otherwise, the check is performed only for debug.
/// warning, this has no effect if lazy is set to true
/// - assertion means that an assertion will be introduced for the check.
/// - eigenAlignedNew should be set to true if any alignment is required for the
/// new operator (see eigen documentation)
template<typename T, bool lazy = false, bool alwaysCheck = false, bool assertion = true, bool eigenAlignedNew = false>
class CheckedItem
{
public:
  /// The parameter initialize sets whether the isSet() parameter is initialized to false
  CheckedItem(bool initialize = true);
  explicit CheckedItem(const T &);
  CheckedItem(const CheckedItem &);
  virtual ~CheckedItem() {}

  inline CheckedItem & operator=(const CheckedItem &);
  inline T & operator=(const T &);
  inline operator T() const;
  inline operator const T &() const;

  inline T chckitm_getValue() const;

  inline T & operator()();
  inline const T & operator()() const;

  inline bool isSet() const;
  inline void reset();

  /// set the value of the initialization check boolean
  inline void set(bool value);

  void setAssertMessage(std::string s);
  void setExceptionPtr(std::exception * e);

  /// allows to set the initialization boolean to true and give a reference
  /// to the object with the same instruction
  /// should be used to initialize the object without using the
  /// assignment operator
  inline T & set();

protected:
  static const bool do_check_ = !lazy || isDebug;
  static const bool do_assert_ = do_check_ && assertion;
  static const bool do_exception_ = do_check_ && !assertion;

  typedef DebugItem<bool, detail::defaultTrue, do_check_> IsSet;
  typedef DebugItem<const char *, detail::defaultErrorMSG, do_assert_> AssertMsg;
  typedef DebugItem<const std::exception *, detail::defaultExceptionAddr, do_exception_> ExceptionPtr;

  IsSet isSet_;
  AssertMsg assertMsg_;
  ExceptionPtr exceptionPtr_;

  bool chckitm_check_() const; /// this can throw(std::exception)
  T v_;

public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(eigenAlignedNew)
};

typedef CheckedItem<Matrix3, false, false, true, true> CheckedMatrix3;
typedef CheckedItem<Matrix6, false, false, true, true> CheckedMatrix6;
typedef CheckedItem<Matrix12, false, false, true, true> CheckedMatrix12;
typedef CheckedItem<Vector3, false, false, true, true> CheckedVector3;
typedef CheckedItem<Vector6, false, false, true, true> CheckedVector6;
typedef CheckedItem<Quaternion, false, false, true, true> CheckedQuaternion;

/**
 * \class    IndexedMatrixT
 * \brief    This class describes a structure composed by a matrix
 *           of a given size and a time-index parameter. It can tell also if
 *           it is initialized or not.
 *
 *
 */
template<typename MatrixType = Matrix, bool lazy = false>
class IndexedMatrixT : protected DebugItem<bool, detail::defaultTrue, !lazy || isDebug>
{
  typedef DebugItem<bool, detail::defaultTrue, !lazy || isDebug> IsSet;

public:
  /// Default constructor
  IndexedMatrixT();

  /// A constructor with a given matrix value and a time index
  IndexedMatrixT(const MatrixType & v, TimeIndex k);

  /// Set the value of the matrix and the time sample
  inline MatrixType set(const MatrixType & v, TimeIndex k);

  /// Switch the vector to "initialized" state
  inline void set(bool value = true);

  /// set the index of the matrix
  inline void setIndex(TimeIndex index);

  /// Get the matrix value (const version)
  inline const MatrixType & operator()() const;

  /// Get the matrix value
  inline MatrixType & operator()();

  /// Get the time index
  inline TimeIndex getTime() const;

  /// Says whether the matrix is initialized or not
  inline bool isSet() const;

  /// Switch off the initalization flag, the value is no longer accessible
  inline void reset();

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

protected:
  /// Checks whether the matrix is set or not (assert)
  /// does nothing in release mode
  inline bool check_() const;
  TimeIndex k_;
  MatrixType v_;
};

typedef IndexedMatrixT<Matrix> IndexedMatrix;
typedef IndexedMatrixT<Vector> IndexedVector;
typedef IndexedMatrixT<Vector3> IndexedVector3;
typedef IndexedMatrixT<Matrix3> IndexedMatrix3;

/**
 * \class    IndexedMatrixArray
 * \brief    This class describes a structure that enables to store array of matrices
 *           with time indexation.
 *
 */

template<typename MatrixType = Matrix, typename Allocator = std::allocator<MatrixType>>
class IndexedMatrixArrayT
{
public:
  /// Default constructor
  IndexedMatrixArrayT();

  typedef std::vector<MatrixType, Allocator> Array;

  /// Sets the vector v at the time index k
  /// It checks the time index, the array must have contiguous indexes
  /// It can be used to push a value into the back of the array
  inline void setValue(const MatrixType & v, TimeIndex k);

  /// Pushes back the matrix to the array, the new value will take the next time
  inline void pushBack(const MatrixType & v);

  /// removes the first (oldest) element of the array
  inline void popFront();

  /// gets the value with the given time index
  inline MatrixType operator[](TimeIndex index) const;

  /// gets the value with the given time index, non const version
  inline MatrixType & operator[](TimeIndex index);

  /// gets the first value
  inline const MatrixType & front() const;

  /// gets the first value
  inline MatrixType & front();

  /// gets the last value
  inline const MatrixType & back() const;

  /// gets the last value
  inline MatrixType & back();

  /// removes all the elements with larger indexes than timeIndex
  void truncateAfter(TimeIndex timeIndex);

  /// removes all the elements with smaller indexes than timeIndex
  void truncateBefore(TimeIndex timeIndex);

  /// resizes the array
  inline void resize(TimeSize i, const MatrixType & m = MatrixType());

  /// Get the time index
  inline TimeIndex getLastIndex() const;

  /// Get the time index of the next value that will be pushed back
  /// Can be used in for loops
  inline TimeIndex getNextIndex() const;

  /// Set the time index of the last element
  inline TimeIndex setLastIndex(int index);

  /// Get the time index
  inline TimeIndex getFirstIndex() const;

  /// set the time index of the first element
  inline TimeIndex setFirstIndex(int index);

  inline TimeSize size() const;

  /// Resets the array to initial state
  /// the value is no longer accessible
  inline void reset();

  /// Clears the vector but keeps the last index
  inline void clear();

  /// converts the array into a standard vector
  Array getArray() const;

  /// checks whether the index is present in the array
  inline bool checkIndex(TimeIndex k) const;

  /// gets the array from a file
  /// the line starts with the time index and then the matrix is read
  /// row by row
  /// WARNING: this resets the array
  void readFromFile(const char * filename, Index rows, Index cols = 1, bool withTimeStamp = true);
  void readFromFile(const std::string & filename, Index rows, Index cols = 1, bool withTimeStamp = true);

  /// gets the array from a file
  /// the line starts with the time index and then every line of the file
  /// is converted into a vector
  /// WARNING: this resets the array
  void readVectorsFromFile(const std::string & filename, bool withTimeStamp = true);
  void readVectorsFromFile(const char * filename, bool withTimeStamp = true);

  /// write the array in a a file
  /// the line starts with the time index and then the matrix is described
  /// row by row
  /// When clear is set, the array is cleared but the time index is conserved
  /// When append is set to true, the output is appended to file
  void writeInFile(const char * filename, bool clear = false, bool append = false);

  /// write the array in a a file
  /// the line starts with the time index and then the matrix is described
  /// row by row
  /// When clear is set, the array is cleared but the time index is conserved
  /// When append is set to true, the output is appended to file
  void writeInFile(const std::string & filename, bool clear = false, bool append = false);

protected:
  typedef std::deque<MatrixType, Allocator> Deque;

  /// Asserts that the index is present in the array
  /// does nothing in release mode
  inline void check_(TimeIndex time) const;

  /// Asserts that the array is not empty
  /// does nothing in release mode
  inline void check_() const;

  /// Asserts that the given time can be pushed at the back of the vector
  /// does nothing in release mode
  inline void checkNext_(TimeIndex time) const;

  TimeIndex k_;

  Deque v_;
};

typedef IndexedMatrixArrayT<Matrix> IndexedMatrixArray;
typedef IndexedMatrixArrayT<Vector> IndexedVectorArray;

namespace cst
{
constexpr double gravityConstant = 9.8;

/// Gravity Vector along Z
const Vector gravity = gravityConstant * Vector3::UnitZ();

/// angles considered Zero
constexpr double epsilonAngle = 1e-16;

} // namespace cst

typedef boost::timer::auto_cpu_timer auto_cpu_timer;
typedef boost::timer::cpu_timer cpu_timer;
typedef boost::timer::cpu_timer cpu_times;

namespace tools
{
struct STATE_OBSERVATION_DLLAPI SimplestStopwatch
{
  /** Always pick a steady clock */
  using clock = typename std::conditional<std::chrono::high_resolution_clock::is_steady,
                                          std::chrono::high_resolution_clock,
                                          std::chrono::steady_clock>::type;
  using time_ns = clock::time_point;
  time_ns startTime;

  inline void start()
  {
    startTime = clock::now();
  }

  /// provides the time since the start
  /// the value is in nanoseconds
  inline double stop()
  {
    auto elapsed = clock::now() - startTime;
    return static_cast<double>(elapsed.count());
  }
};

std::string STATE_OBSERVATION_DLLAPI matrixToString(const Matrix & mat);

std::string STATE_OBSERVATION_DLLAPI vectorToString(const Vector & v);

Matrix STATE_OBSERVATION_DLLAPI stringToMatrix(const std::string & str, Index rows, Index cols);

Vector STATE_OBSERVATION_DLLAPI stringToVector(const std::string & str, Index length);

Vector STATE_OBSERVATION_DLLAPI stringToVector(const std::string & str);
} // namespace tools

#include <state-observation/tools/definitions.hxx>
} // namespace stateObservation

#endif // STATEOBSERVATIONDEFINITIONSHPP


///////////////////////////////////////////////////////////////////////////////
//  Copyright Christopher Kormanyos 2015.
//  Copyright Nikhar Agrawal 2015.
//  Copyright Paul Bristow 2015.
//  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)

//! \file
//!\brief Tests for the nearbyint function of (fixed_point).

#include <cmath>

#define BOOST_TEST_MODULE test_negatable_func_nearbyint
#define BOOST_LIB_DIAGNOSTIC

#include <boost/fixed_point/fixed_point.hpp>
#include <boost/test/included/unit_test.hpp>

BOOST_AUTO_TEST_CASE(test_negatable_func_nearbyint)
{
  using std::nearbyint;

  // Test nearbyint(x) for round mode round::nearest_even.
  {
    typedef boost::fixed_point::negatable<7, -8, boost::fixed_point::round::nearest_even> fixed_point_type;
    typedef fixed_point_type::float_type                                                  float_point_type;

    fixed_point_type x;
    float_point_type y;

    x = nearbyint(fixed_point_type(1.5F));
    y = nearbyint(float_point_type(1.5F));

    BOOST_CHECK_EQUAL(x, fixed_point_type(y));

    x = nearbyint(fixed_point_type(-1.5F));
    y = nearbyint(float_point_type(-1.5F));

    BOOST_CHECK_EQUAL(x, fixed_point_type(y));
  }

  // Test nearbyint(x) for round mode round::fastest.
  {
    typedef boost::fixed_point::negatable<7, -8, boost::fixed_point::round::fastest> fixed_point_type;
    typedef fixed_point_type::float_type                                             float_point_type;

    fixed_point_type x;
    float_point_type y;

    x = nearbyint(fixed_point_type(1.5F));
    y = nearbyint(float_point_type(1.0F));

    BOOST_CHECK_EQUAL(x, fixed_point_type(y));

    x = nearbyint(fixed_point_type(-1.5F));
    y = nearbyint(float_point_type(-1.0F));

    BOOST_CHECK_EQUAL(x, fixed_point_type(y));
  }
}


//=======================================================================
// Copyright (c) 2014 Andrzej Pacuk
//
// 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)
//=======================================================================
/**
 * @file lsh_nearest_neighbors_regression_example.cpp
 * @brief
 * @author Andrzej Pacuk
 * @version 1.0
 * @date 2014-10-06
 */

//! [LSH Nearest Neighbors Regression Example]
#include "paal/regression/lsh_nearest_neighbors_regression.hpp"

#include <boost/numeric/ublas/vector.hpp>
#include <boost/range/algorithm/copy.hpp>

#include <iostream>
#include <iterator>
#include <vector>

using coordinate_t = int;
using point_t = boost::numeric::ublas::vector<coordinate_t>;

auto make_point(const std::initializer_list<int> &list) {
    point_t point(list.size());
    boost::copy(list, point.begin());
    return point;
};

int main() {
    const std::vector<point_t> training_points =
            {make_point({0, 0}), make_point({0, 1}),
             make_point({1, 0}), make_point({1, 1})};
    const std::vector<double> training_results = {0.0, 0.4, 0.6, 1.0};
    const std::vector<point_t> test_points =
            {make_point({0, -1}), make_point({2, 1})};

    auto const passes = 50;
    auto const hash_functions_per_point = 10;
    auto const dimensions = training_points.front().size();
    auto const threads_count = 1;
    //w_param should be essentially bigger than radius of expected test point neighborhood
    auto const w_param = 3.0;

    auto lsh_function_generator =
        paal::lsh::l_2_hash_function_generator<>{dimensions, w_param};
    auto model = paal::make_lsh_nearest_neighbors_regression_tuple_hash(
                    training_points, training_results,
                    passes,
                    std::move(lsh_function_generator),
                    hash_functions_per_point,
                    threads_count);

    std::vector<double> results;
    model.test(test_points, std::back_inserter(results));

    std::cout << "Solution:" << std::endl;
    boost::copy(results, std::ostream_iterator<double>(std::cout, ","));
    std::cout << std::endl;

    return 0;
}
//! [LSH Nearest Neighbors Regression Example]


//==================================================================================================
/*!

  Copyright 2015 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)
*/
//==================================================================================================
#include <boost/simd/function/scalar/min.hpp>
#include <simd_test.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/constant/two.hpp>

STF_CASE_TPL (" min real",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  using bs::min;

  // return type conformity test
   STF_EXPR_IS(min(T(),T()), T);

  // specific values tests
#ifndef STF_NO_INVALIDS
  STF_EQUAL(min(bs::Inf<T>(), bs::Inf<T>()), bs::Inf<T>());
  STF_EQUAL(min(bs::Minf<T>(), bs::Minf<T>()), bs::Minf<T>());
  STF_IEEE_EQUAL(min(bs::Nan<T>(), bs::Nan<T>()), bs::Nan<T>());
#endif
  STF_EQUAL(min(bs::Mone<T>(), bs::Mone<T>()), bs::Mone<T>());
  STF_EQUAL(min(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(min(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(min(bs::Nan<T>(), bs::One<T>()), bs::One<T>());
  STF_IEEE_EQUAL(min(bs::One<T>(), bs::Nan<T>()), bs::Nan<T>());
  STF_EQUAL(min(bs::One<T>(), bs::Two<T>()), bs::One<T>());
  STF_EQUAL(min(bs::Two<T>(), bs::One<T>()), bs::One<T>());
} // end of test for floating_

STF_CASE_TPL (" minu nsigned int",  STF_UNSIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::min;

  // return type conformity test
  STF_EXPR_IS(min(T(),T()), T);

  // specific values tests
  STF_EQUAL(min(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(min(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(min(bs::One<T>(), bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(min(bs::Zero<T>(), bs::One<T>()), bs::Zero<T>());
} // end of test for unsigned_int_

STF_CASE_TPL (" min signed int",  STF_SIGNED_INTEGRAL_TYPES)
{
  namespace bs = boost::simd;
  using bs::min;

  // return type conformity test
  STF_EXPR_IS(min(T(),T()), T);

  // specific values tests
  STF_EQUAL(min(bs::Mone<T>(), bs::Mone<T>()), bs::Mone<T>());
  STF_EQUAL(min(bs::One<T>(), bs::One<T>()), bs::One<T>());
  STF_EQUAL(min(bs::Zero<T>(), bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(min(bs::One<T>(), bs::Zero<T>()), bs::Zero<T>());
  STF_EQUAL(min(bs::Zero<T>(), bs::One<T>()), bs::Zero<T>());
  STF_EQUAL(min(bs::Mone<T>(), bs::Zero<T>()), bs::Mone<T>());
  STF_EQUAL(min(bs::Zero<T>(), bs::Mone<T>()), bs::Mone<T>());
} // end of test for signed_int_


// -*- mode: c++; indent-tabs-mode: nil; -*-
//
// Paragraph
// Copyright (c) 2016-2019 Illumina, Inc.
// All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//		http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied
// See the License for the specific language governing permissions and limitations
//
//

#include "genotyping/BreakpointGenotyper.hh"
#include "common/Error.hh"
#include <algorithm>
#include <boost/math/distributions/normal.hpp>
#include <boost/math/distributions/poisson.hpp>
#include <boost/math/special_functions/binomial.hpp>
#include <cmath>
#include <limits>
#include <math.h>
#include <numeric>

using boost::math::normal_distribution;
using boost::math::poisson_distribution;
using std::map;
using std::vector;

namespace genotyping
{

/**
 * @param genotype_parameters GenotypeParameter class
 */
BreakpointGenotyper::BreakpointGenotyper(std::unique_ptr<GenotypingParameters> const& param)
    : n_alleles_(param->numAlleles())
    , ploidy_(param->ploidy())
    , coverage_test_cutoff_(param->coverageTestCutoff())
    , min_pass_gq_(param->minPassGQ())
    , min_overlap_bases_(param->minOverlapBases())
    , possible_genotypes(param->possibleGenotypes())
{
    if (param->alleleErrorRates().empty())
    {
        allele_error_rate_.push_back(param->otherAlleleErrorRate());
    }
    else
    {
        allele_error_rate_ = param->alleleErrorRates();
    }

    if (param->hetHaplotypeFractions().empty())
    {
        haplotype_read_fraction_.push_back(param->otherHetHaplotypeFraction());
    }
    else
    {
        haplotype_read_fraction_ = param->hetHaplotypeFractions();
    }

    if (!param->genotypeFractions().empty())
    {
        genotype_prior_ = param->genotypeFractions();
        for (auto& phi : genotype_prior_)
        {
            if (phi.first.size() < ploidy_)
            {
                error("Error: genotype and ploidy does not match.");
            }
            if (phi.second < 0 || phi.second > 1)
            {
                error("Error: genotype prior should be between 0~1.");
            }
            phi.second = log(phi.second);
        }
    }
};

Genotype BreakpointGenotyper::genotype(
    const BreakpointGenotyperParameter& param, const std::vector<int32_t>& read_counts_per_allele) const
{
    if (read_counts_per_allele.size() != n_alleles_)
    {
        error(
            "Error: number of read counts and alleles mismatches. %i != %i.", (int)read_counts_per_allele.size(),
            n_alleles_);
    }
    Genotype result;

    // compute adjusted depth
    const double multiplier = (param.read_length - min_overlap_bases_) / (double)param.read_length;
    assert(multiplier > 0);
    const double lambda = param.read_depth * multiplier;
    const int32_t total_num_reads = std::accumulate(read_counts_per_allele.begin(), read_counts_per_allele.end(), 0);
    if (total_num_reads == 0)
    {
        result.filters.insert("NO_READS");
        return result;
    }
    result.num_reads = total_num_reads;

    // compute GL and GT
    double best_gl = -std::numeric_limits<double>::max();

    for (const auto& igt : possible_genotypes)
    {
        const double gl = genotypeLikelihood(lambda, igt, read_counts_per_allele);
        result.gl_name.push_back(igt);
        result.gl.push_back(gl);

        // update GT if GL is better
        if (gl > best_gl)
        {
            best_gl = gl;
            result.gt = igt;
        }
    }

    // compute GQ and set filter
    double sum_gl = 0;
    for (auto l : result.gl)
    {
        sum_gl += exp(l);
    }
    double pr_gt_error = (double)1.0 - exp(best_gl) / sum_gl;
    if (pr_gt_error == 0)
    {
        result.gq = 100;
    }
    else
    {
        double gq_log10 = log10(pr_gt_error);
        if (gq_log10 < -10)
        {
            result.gq = 100;
        }
        else
        {
            result.gq = -10 * gq_log10;
        }
    }
    if (result.gq < min_pass_gq_)
    {
        result.filters.insert("GQ");
    }

    // compute allele fractions
    result.allele_fractions.resize(n_alleles_, 0.0);
    for (unsigned int al = 0; al < n_alleles_; ++al)
    {
        result.allele_fractions[al] = ((double)read_counts_per_allele[al]) / total_num_reads;
    }

    // compute coverage test p value
    double coverage_test_pvalue;
    if (param.use_poisson_depth) // use poisson test for depth (more stringent)
    {
        const poisson_distribution<> poisson_coverage_distribution(lambda);
        coverage_test_pvalue = cdf(poisson_coverage_distribution, total_num_reads);
    }
    else // use normal test for depth (default)
    {
        const normal_distribution<> normal_coverage_distribution(lambda, param.depth_sd);
        coverage_test_pvalue = cdf(normal_coverage_distribution, total_num_reads);
    }

    if (coverage_test_pvalue > 0.5)
    {
        coverage_test_pvalue = 1 - coverage_test_pvalue;
        if (coverage_test_pvalue < coverage_test_cutoff_.first)
        {
            result.filters.insert("BP_DEPTH");
        }
    }
    else
    {
        if (coverage_test_pvalue < coverage_test_cutoff_.second)
        {
            result.filters.insert("BP_DEPTH");
        }
    }
    result.coverage_test_pvalue = coverage_test_pvalue;

    return result;
}

/**
 * return genotype likelihood given genotype (using Poisson model with internal parameters)
 * @param lambda Poisson distribution parameter
 * @param gv Genotype vector
 * @param read_counts Read count vector for each allele
 */
double BreakpointGenotyper::genotypeLikelihood(
    double lambda, const GenotypeVector& gv, const vector<int32_t>& read_counts) const
{
    double log_phi;
    auto it = genotype_prior_.find(gv);
    if (it == genotype_prior_.end())
    {
        log_phi = 0;
    }
    else
    {
        log_phi = it->second;
    }

    // compute how many copies of each allele we have in this GT
    vector<int> allele_ploidy;
    allele_ploidy.resize(n_alleles_, 0);
    for (unsigned int al = 0; al < n_alleles_; ++al)
    {
        for (const auto g : gv)
        {
            if (al == g)
            {
                ++allele_ploidy[al];
            }
        }
    }

    // compute GL by summing all allele contributions
    double gl = log_phi;
    for (unsigned int al = 0; al < n_alleles_; ++al)
    {
        if (allele_ploidy[al] == 0)
        {
            // no copies -> all reads supporting this allele will be errors
            const double eps = (allele_error_rate_.size() == 1 ? allele_error_rate_[0] : allele_error_rate_[al]);
            const poisson_distribution<> error_distribution(lambda * eps);
            gl += log(pdf(error_distribution, read_counts[al]));
        }
        else
        {
            const double mu
                = (haplotype_read_fraction_.size() == 1 ? haplotype_read_fraction_[0] : haplotype_read_fraction_[al]);
            const double mu_with_ploidy = allele_ploidy[al] * mu;

            const poisson_distribution<> allele_count_distribution(lambda * mu_with_ploidy);
            gl += log(pdf(allele_count_distribution, read_counts[al]));
        }
        if (std::isinf(gl))
        {
            return -std::numeric_limits<double>::max();
        }
    }

    return gl;
}
}


#include <string>
#include <sstream>
#include <iostream>
#include <vector>
#include <list>
#include <math.h>
#include <vtkSmartPointer.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyDataWriter.h>
#include <vtkPolyData.h>
#include <vtkCellData.h>
#include <vtkPointData.h>
#include <vtkCell.h>
#include <vtkDoubleArray.h>
#include <vtkKdTree.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <omp.h>
//#include "HelperFunctions.h"

using namespace std;
//using namespace OPS;

typedef Eigen::Vector3d Vector3d;

//! A struct to store a vtkIdType and an angle
struct neighbors{
    vtkIdType _id;
    double_t _angle;
    neighbors(vtkIdType i, double_t a):_id(i), _angle(a){}
    bool operator <(const neighbors &n) const{
	return _angle < n._angle;
    }
};

int main(int argc, char* argv[])
{
    if( argc != 5 ) {
	cout << "argc = " << argc << endl
	    << "Usage: dualMesh baseName numStart numEnd numOPENMPThreads"
	    << endl;
	return(0);
    }

    ////////////////////////////////////////////////////////////////////
    // Input section
    ////////////////////////////////////////////////////////////////////

    // read in vtk file
    string baseFileName = argv[1];
    size_t numStart = stoi(argv[2]);
    size_t numEnd = stoi(argv[3]);
    size_t numThreads = stoi(argv[4]);

    //Set the number of threads
    omp_set_num_threads(numThreads);

#pragma omp parallel for
    for(size_t bigI=numStart; bigI <= numEnd; bigI++){
	stringstream sstm;
	string inputFileName, outFileName;
	sstm << baseFileName << "-" << bigI <<".vtk";
	inputFileName = sstm.str();
	sstm.str("");
	sstm.clear();

	auto reader = vtkSmartPointer<vtkPolyDataReader>::New();
	reader->SetFileName( inputFileName.c_str() );
	reader->Update();
	vtkSmartPointer<vtkPolyData> inputMesh = reader->GetOutput();
	inputMesh->BuildLinks();
	size_t npts = inputMesh->GetNumberOfPoints();

	//get displacement vectors, if they exist
	string vectorName="displacements";
	vtkSmartPointer<vtkDoubleArray> displacements =
	    vtkDoubleArray::SafeDownCast(inputMesh->GetPointData()->
		    GetVectors(vectorName.c_str()));

	// get vertex positions
	std::vector< Vector3d > points( npts, Vector3d::Zero() );

	// Calculate centroid of each triangle while updating points vector
	auto newPts = vtkSmartPointer<vtkPoints>::New();
	vtkSmartPointer<vtkCellArray> cells = inputMesh->GetPolys();
	auto cellPointIds = vtkSmartPointer<vtkIdList>::New();
	cells->InitTraversal();
	while( cells->GetNextCell( cellPointIds ) ){
	    size_t numCellPoints = cellPointIds->GetNumberOfIds();
	    Vector3d centroid(0.0,0.0,0.0);
	    for(size_t i=0; i < numCellPoints; i++){
		vtkIdType currCellPoint = cellPointIds->GetId(i);
		inputMesh->GetPoint( currCellPoint, &points[currCellPoint][0] );
		if( displacements.GetPointer() != NULL){
		    Vector3d currDisp(0.0,0.0,0.0);
		    displacements->GetTuple( currCellPoint, &currDisp[0] );
		    points[currCellPoint] += currDisp;
		}
		centroid += points[currCellPoint];
	    }
	    centroid /= numCellPoints;
	    newPts->InsertNextPoint( &centroid[0] );
	}

	// Prepare valence Cell Data array
	auto valence = vtkSmartPointer<vtkIntArray>::New();
	valence->SetName("Valence");
	valence->SetNumberOfComponents(1);

	// Prepare new cell array for polygons
	auto newPolys = vtkSmartPointer<vtkCellArray>::New();

	for(size_t a=0; a < npts; a++) {

	    auto currPolyPtIds = vtkSmartPointer<vtkIdList>::New();
	    std::list< neighbors > currPoly;
	    Vector3d vec0, vecj, currCross, axis, centroid(0.0,0.0,0.0);
	    double vec0_norm, vecj_norm, sign, currSin, currCos, currAngle;

	    inputMesh->GetPointCells( a, currPolyPtIds );
	    size_t numCellPoints = currPolyPtIds->GetNumberOfIds();

	    // Get coordinates of first cell's centroid
	    vtkIdType currId = currPolyPtIds->GetId(0);
	    newPts->GetPoint( currId, &centroid[0] );
	    vec0 = (centroid - points[a]).normalized();
	    neighbors pt0(currId, 0.0);
	    currPoly.push_back( pt0 );

	    // For remaining centroids
	    for(auto j=1; j < numCellPoints; j++){
		currId = currPolyPtIds->GetId(j);
		newPts->GetPoint( currId, &centroid[0] );
		vecj = (centroid - points[a]).normalized();
		currSin = (vec0.cross( vecj )).norm();
		axis = (vec0.cross( vecj )).normalized();
		sign = axis.dot( points[a] );
		currSin = (sign > 0.0) ? currSin : -1.0 * currSin;
		currCos = vec0.dot( vecj );
		currAngle = (180 / M_PI) * atan2(currSin, currCos);
		currAngle = (currAngle < 0) ? (360 + currAngle) : currAngle;
		neighbors ptj(currId, currAngle);
		currPoly.push_back(ptj);
	    }

	    //Sort the list of neigbors and make a polygon
	    currPoly.sort();
	    newPolys->InsertNextCell( numCellPoints );
	    for(auto t = currPoly.begin(); t != currPoly.end(); ++t){
		neighbors n = *t;
		newPolys->InsertCellPoint( n._id );
	    }
	    valence->InsertNextTuple1( numCellPoints );
	}

	//Assign points and polygons to a new polydata and write it out
	auto newPolyData = vtkSmartPointer<vtkPolyData>::New();
	auto writer = vtkSmartPointer<vtkPolyDataWriter>::New();

	newPolyData->SetPoints( newPts );
	newPolyData->SetPolys( newPolys );
	newPolyData->GetCellData()->AddArray( valence );
	writer->SetInputData( newPolyData );
	sstm << baseFileName << "-dual-" << bigI <<".vtk";
	outFileName = sstm.str();
	sstm.str("");
	sstm.clear();
	writer->SetFileName(outFileName.c_str());
	writer->Write();
    }
    return 0;
}


#include <boost/polygon/voronoi.hpp>