#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <control_toolbox/pid.h>
#include <realtime_tools/realtime_buffer.h>

#include <pluginlib/class_list_macros.h>

#include "std_msgs/MultiArrayLayout.h"
#include "std_msgs/MultiArrayDimension.h"

#include <std_msgs/Float64MultiArray.h>
#include <angles/angles.h>
#include <geometry_msgs/WrenchStamped.h>

#include <urdf/model.h>

#include <kdl/tree.hpp>
#include <kdl/kdl.hpp>
#include <kdl/chain.hpp>
#include <kdl/chaindynparam.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainiksolvervel_pinv.hpp>
#include <kdl/chainiksolverpos_nr_jl.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <tf_conversions/tf_kdl.h>

#include <boost/scoped_ptr.hpp>

// from computed torque clik
#include <boost/lexical_cast.hpp>
//
#include <math.h>
#include <Eigen/LU>
#include <utils/pseudo_inversion.h>
#include <utils/skew_symmetric.h>

//#define SaveDataMax 97
#define num_taskspace 6
#define A 0.1
#define b 2.5
#define f 1
#define t_set 1

#include <string>
#include <iostream>
//

#define PI 3.141592
#define D2R PI/180.0
#define R2D 180.0/PI
#define JointMax 6
#define SaveDataMax 7

namespace arm_controllers{

class AdmittanceController: public controller_interface::Controller<hardware_interface::EffortJointInterface>
{
public:
    ~AdmittanceController() {sub_q_cmd_.shutdown(); sub_forcetorque_sensor_.shutdown();}

    bool init(hardware_interface::EffortJointInterface* hw, ros::NodeHandle &n)
    {
        // List of controlled joints
        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;
        }

        // urdf
        urdf::Model urdf;
        if (!urdf.initParam("elfin/robot_description"))
        {
            ROS_ERROR("Failed to parse urdf file");
            return false;
        }

        // joint handle
        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);
        }

        // kdl parser
        if (!kdl_parser::treeFromUrdfModel(urdf, kdl_tree_)){
            ROS_ERROR("Failed to construct kdl tree");
            return false;
        }

        // 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;
        }

        gravity_ = KDL::Vector::Zero();
        gravity_(2) = -9.81;
        G_.resize(n_joints_);

        // inverse dynamics solver
        id_solver_.reset( new KDL::ChainDynParam(kdl_chain_, gravity_) );
        fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
        ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(kdl_chain_));
        //ik_pos_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(kdl_chain_, fk_solver_, ik_vel_solver_));

        // command and state
        tau_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
        tau_cmd_old_.data = Eigen::VectorXd::Zero(n_joints_);
        q_cmd_sp_.data = Eigen::VectorXd::Zero(n_joints_);
        q_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
        q_cmd_old_.data = Eigen::VectorXd::Zero(n_joints_);
        qdot_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
        qdot_cmd_old_.data = Eigen::VectorXd::Zero(n_joints_);
        qddot_cmd_.data = Eigen::VectorXd::Zero(n_joints_);
        q_cmd_end_.data = Eigen::VectorXd::Zero(n_joints_);

        q_.data = Eigen::VectorXd::Zero(n_joints_);
        q_init_.data = Eigen::VectorXd::Zero(n_joints_);
        qdot_.data = Eigen::VectorXd::Zero(n_joints_);
        qdot_old_.data = Eigen::VectorXd::Zero(n_joints_);
        qddot_.data = Eigen::VectorXd::Zero(n_joints_);

        for (size_t i = 0; i < 6; i++)
        {
            Xc_dot_(i) = 0.0;
            Xc_dot_old_(i) = 0.0;
            Xc_ddot_(i) = 0.0;
        }

        // gains
        Mbar_.resize(n_joints_);
        Mbar_dot_.resize(n_joints_);
        Ramda_.resize(n_joints_);
        Alpha_.resize(n_joints_);
        Omega_.resize(n_joints_);

        Xr_dot_ = 0.0;
        Xe_dot_ = 0.0;
        Xe_ddot_ = 0.0;
        Fd_ = 0.0;
        Fd_temp_ = 0.0;
        Fd_old_ = 0.0;
        Fe_ = 0.0;
        Fe_old_ = 0.0;
        M_ = 0.0;
        B_ = 0.0;
        del_B_ = 0.0;
        B_buffer_ = 0.0;
        PI_ = 0.0;
        PI_old_ = 0.0;

        filt_old_ = 0.0;
        filt_ = 0.0;
        tau_ = 1.0/(2*PI*9.0);

        f_cur_buffer_ = 0.0;

        experiment_mode_ = 0;

        std::vector<double> Mbar(n_joints_), Ramda(n_joints_), Alpha(n_joints_), Omega(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/admittance_controller/joint" + si + "/tdc/mbar", Mbar[i]) )
            {
                Mbar_(i) = Mbar[i];
            }
            else
            {
                std::cout << "/elfin/admittance_controller/joint" + si + "/tdc/mbar" << std::endl;
                ROS_ERROR("Cannot find tdc/mbar gain");
                return false;
            }

            if ( n.getParam("/elfin/admittance_controller/joint" + si + "/tdc/r", Ramda[i]) )
            {
                Ramda_(i) = Ramda[i];
            }
            else
            {
                ROS_ERROR("Cannot find tdc/r gain");
                return false;
            }

            if ( n.getParam("/elfin/admittance_controller/joint" + si + "/tdc/a", Alpha[i]) )
            {
                Alpha_(i) = Alpha[i];
            }
            else
            {
                ROS_ERROR("Cannot find tdc/a gain");
                return false;
            }

            if ( n.getParam("/elfin/admittance_controller/joint" + si + "/tdc/w", Omega[i]) )
            {
                Omega_(i) = Omega[i];
            }
            else
            {
                ROS_ERROR("Cannot find tdc/w gain");
                return false;
            }
        }

        if (!n.getParam("/elfin/admittance_controller/aic/fd", Fd_temp_))
        {
            ROS_ERROR("Cannot find aci/fd");
            return false;
        }

        if (!n.getParam("/elfin/admittance_controller/aic/m", M_))
        {
            ROS_ERROR("Cannot find aci/m");
            return false;
        }

        if (!n.getParam("/elfin/admittance_controller/aic/b", B_))
        {
            ROS_ERROR("Cannot find aci/b");
            return false;
        }

        if (!n.getParam("/elfin/admittance_controller/mode", experiment_mode_))
        {
            ROS_ERROR("Cannot find mode");
            return false;
        }

        // command
        sub_q_cmd_ = n.subscribe("command", 1, &AdmittanceController::commandCB, this);
        sub_forcetorque_sensor_ = n.subscribe<geometry_msgs::WrenchStamped>("/elfin/elfin/ft_sensor_topic", 1, &AdmittanceController::updateFTsensor, this);

        pub_SaveData_ = n.advertise<std_msgs::Float64MultiArray>("SaveData", 1000);

        // ---------------------from Control clik-------------

        // ********* 1.  joint name / gain from the parameter server *********
        // 1.0 Control objective & Inverse Kinematics mode
        if (!n.getParam("ctr_obj", cctr_obj_))
        {
            ROS_ERROR("Could not find control objective");
            return false;
        }

        if (!n.getParam("ik_mode", iik_mode_))
        {
            ROS_ERROR("Could not find control objective");
            return false;
        }

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

        // had to subtract 1 manually. Sorry!
        nn_joints_ = jjoint_names_.size()-1;

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

        // 1.2 Gain
        // 1.2.1 Joint Controller
        KKp_.resize(nn_joints_);
        KKd_.resize(nn_joints_);
        KKi_.resize(nn_joints_);

        std::vector<double> KKp(nn_joints_), KKi(nn_joints_), KKd(nn_joints_);

        for (size_t i = 0; i < nn_joints_; i++)
        {
            ROS_INFO("%d",i);
            std::string si = boost::lexical_cast<std::string>(i + 1);
            if (n.getParam("/elfin/admittance_controller/gains/elfin_joint" + si + "/pid/p", KKp[i]))
            {
                KKp_(i) = KKp[i];
            }
            else
            {
                std::cout << "/elfin/admittance_controller/gains/elfin_joint" + si + "/pid/p" << std::endl;
                ROS_ERROR("Cannot find pid/p gain");
                return false;
            }

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

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

        // 1.2.2 Closed-loop Inverse Kinematics Controller
        if (cctr_obj_ == 1)
        {
            if (!n.getParam("/elfin/admittance_controller/clik_gain/K_regulation", KK_regulation_))
            {
                ROS_ERROR("Cannot find clik regulation gain");
                return false;
            }
        }

        else if (cctr_obj_ == 2)
        {
            if (!n.getParam("/elfin/admittance_controller/clik_gain/K_tracking", KK_tracking_))
            {
                ROS_ERROR("Cannot find clik tracking gain");
                return false;
            }
        }
        /*
        // 2. ********* urdf *********
        urdf::Model uurdf;
        if (!uurdf.initParam("elfin/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 < nn_joints_; i++)
        {
            try
            {
                jjoints_.push_back(hw->getHandle(jjoint_names_[i]));
            }
            catch (const hardware_interface::HardwareInterfaceException &e)
            {
                ROS_ERROR_STREAM("Exception thrown: " << e.what());
                return false;
            }

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

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

        // 4.2 kdl chain
        std::string rroot_name, ttip_name;

        rroot_name="world";
        ttip_name="elfin_link6";

        /*
        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(rroot_name, ttip_name, kkdl_chain_))
        {
            ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
            ROS_ERROR_STREAM("  " << rroot_name << " --> " << ttip_name);
            ROS_ERROR_STREAM("  Tree has " << kkdl_tree_.getNrOfJoints() << " joints");
            ROS_ERROR_STREAM("  Tree has " << kkdl_tree_.getNrOfSegments() << " segments");
            ROS_ERROR_STREAM("  The segments are:");

            KDL::SegmentMap ssegment_map = kkdl_tree_.getSegments();
            KDL::SegmentMap::iterator it;

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

            return false;
        }
        else
        {
            ROS_INFO("Got kdl chain");
        }

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

        iid_solver_.reset(new KDL::ChainDynParam(kkdl_chain_, gravity_));

        // 4.4 jacobian solver 초기화
        jjnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kkdl_chain_));

        // 4.5 forward kinematics solver 초기화
        ffk_pos_solver_.reset(new KDL::ChainFkSolverPos_recursive(kkdl_chain_));

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

        // 5.1 KDL Vector 초기화 (사이즈 정의 및 값 0)
        ttau_d_.data = Eigen::VectorXd::Zero(nn_joints_);
        xx_cmd_.data = Eigen::VectorXd::Zero(num_taskspace);

        qqd_.data = Eigen::VectorXd::Zero(nn_joints_);
        qqd_dot_.data = Eigen::VectorXd::Zero(nn_joints_);
        qqd_ddot_.data = Eigen::VectorXd::Zero(nn_joints_);
        qqd_old_.data = Eigen::VectorXd::Zero(nn_joints_);

        qq_.data = Eigen::VectorXd::Zero(nn_joints_);
        qqdot_.data = Eigen::VectorXd::Zero(nn_joints_);

        ee_.data = Eigen::VectorXd::Zero(nn_joints_);
        ee_dot_.data = Eigen::VectorXd::Zero(nn_joints_);
        ee_int_.data = Eigen::VectorXd::Zero(nn_joints_);

        // 5.2 KDL Matrix 초기화 (사이즈 정의 및 값 0)
        JJ_.resize(nn_joints_);
        // J_inv_.resize(kdl_chain_.getNrOfJoints());
        MM_.resize(nn_joints_);
        CC_.resize(nn_joints_);
        GG_.resize(nn_joints_);

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

        ppub_xd_ = n.advertise<std_msgs::Float64MultiArray>("xd", 1000);
        ppub_x_ = n.advertise<std_msgs::Float64MultiArray>("x", 1000);
        ppub_ex_ = n.advertise<std_msgs::Float64MultiArray>("ex", 1000);

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

        // 6.2 subsriber
        //ssub_x_cmd_ = n.subscribe<std_msgs::Float64MultiArray>("command", 1, &Computed_Torque_Controller_CLIK::commandCB, this);
        eevent = 0; // subscribe 받기 전: 0
        // subscribe 받은 후: 1

        mmsg_x_.layout.dim.push_back(std_msgs::MultiArrayDimension());

        mmsg_x_.data.resize(6);

        for(int i=0;i<6;i++){
            mmsg_x_.data[i]=0;
        }

        k_stiff_damp_sub = n.subscribe<std_msgs::Float64MultiArray>("admittance_gains", 1, &AdmittanceController::admit_gains, this);

        // admittance params
        z=0;
        zdot=0;
        zddot=0;
        z_pre=0;
        zdot_pre=0;
        zddot_pre=0;
        K_damp=20;
        K_stiff=50;
        M_ad=5;

        publisher_ = n.advertise<std_msgs::Float64MultiArray>("publish_admittance", 1000);

        msg_temp.data.resize(6);

        for(int i=0;i<6;i++){
            msg_temp.data[i]=0;
        }
        return true;
    } // end of initialize

    void admit_gains(const std_msgs::Float64MultiArrayConstPtr &msg)
    {
        K_stiff = msg->data[0];
        K_damp  = msg->data[1];
        M_ad= msg->data[2];
    }

    void starting(const ros::Time& time)
    {
        // get joint positions
        for(size_t i=0; i<n_joints_; i++)
        {
            ROS_INFO("JOINT %d", (int)i);
            q_(i) = joints_[i].getPosition();
            q_init_(i) = q_(i);
            qdot_(i) = joints_[i].getVelocity();
        }

        time_ = 0.0;
        total_time_ = 0.0;

        ROS_INFO("Starting Adaptive Impedance Controller");
    }

    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;
        }

        for (unsigned int i = 0; i<n_joints_; i++)
            q_cmd_sp_(i) = msg->data[i];
    }

    //void updateFTsensor(const geometry_msgs::WrenchStamped::ConstPtr &msg)
    void updateFTsensor(const geometry_msgs::WrenchStamped::ConstPtr &msg)
    {
        // Convert Wrench msg to KDL wrench
        geometry_msgs::Wrench f_meas = msg->wrench;

        f_cur_[0] = f_meas.force.x;
        f_cur_buffer_ = f_meas.force.y;
        f_cur_[2] = f_meas.force.z;
        f_cur_[3] = f_meas.torque.x;
        f_cur_[4] = f_meas.torque.y;
        f_cur_[5] = f_meas.torque.z;

        if (experiment_mode_ == 1 || experiment_mode_ == 2)
            f_cur_[1] = first_order_lowpass_filter();
    }

    // load gain is not permitted during controller loading?
    void loadGainCB()
    {

    }

    void update(const ros::Time& time, const ros::Duration& period)
    {
        //---------------------

        // ********* 0. Get states from gazebo *********
        // 0.1 sampling time
        double ddt = period.toSec();
        tt = tt + 0.001;

        //----------------------------

        // simple trajectory interpolation from joint command setpoint
        dt_ = period.toSec();

        if(total_time_ < 5.0)
        {
            task_init();
        }
        else if(total_time_ >= 5.0 && total_time_ < 6.0)
        {
            task_via();
        }
        else if(total_time_ >= 6.0 && total_time_ < 16.0)
        {
            task_ready();
        }
        else if (total_time_ >= 16.0 && total_time_ < 17.0)
        {
            task_via();
        }
        else if (total_time_ >= 17.0 && total_time_ < 27.0)
        {
            task_freespace();
        }
        else if (total_time_ >= 27.0 && total_time_ < 28.0)
        {
            task_via();
            //        }
            //        else if (total_time_ >= 28.0 && total_time_ < 48.0)
            //        {
            //            //task_contactspace();
        }
        else if (total_time_ >= 48.0 && total_time_ < 49.0)
        {
            task_via();
        }
        else if (total_time_ >= 49.0 && total_time_ < 59.0)
        {
            task_homming();
        }

        // get joint states
        for (size_t i=0; i<n_joints_; i++)
        {
            q_(i) = joints_[i].getPosition();
            qdot_(i) = joints_[i].getVelocity();
        }

        //fk_solver_->JntToCart(qq_, xx_);

        for (size_t i=0; i<nn_joints_; i++)
        {
            qq_(i) = jjoints_[i].getPosition();
            qqdot_(i) = jjoints_[i].getVelocity();
        }

        // 0.3 end-effector state by Compute forward kinematics
        ffk_pos_solver_->JntToCart(qq_, xx_);

        for(int i=0 ; i<3; i++){
            mmsg_x_.data[i]=xx_.p(i);
        }

        ppub_x_.publish(mmsg_x_);

        //        if (total_time_ >= 28.0 && total_time_ < 48.0)
        if (total_time_ >= 28.0)
        {

            if (flag!=2)
            {
                xxd_= xx_;
                xx=xx_.p(0)-0.02;
                //yy=xx_.p(1);
                zz=xx_.p(2)-0.05;

                xxd_dot_(0) = 0;
                xxd_dot_(1) = 0;
                xxd_dot_(2) = 0;
                xxd_dot_(3) = 0;
                xxd_dot_(4) = 0;
                xxd_dot_(5) = 0;

                xxd_.p(2)=zz;

                flag = 2;
            }

            double amp=0.18;
            double ttt=(total_time_-28)/30;
            ttt=ttt-floor(ttt);
            xxd_.p(0) = xx+amp*sin(PI*ttt);

            //xxd_dot_(0) = amp*PI*cos(PI*ttt);
            //xxd_ddot_(0)=-1*amp*PI*PI*sin(PI*ttt);

            // ADMITTACE CONTROL IMPLEMENTATION***********************************************************

            double F =abs(f_cur_[1]);
            //double F =f_cur_[1];

            zddot= (F-K_damp*zdot_pre-K_stiff*z_pre)/M_ad;
            zdot=zdot_pre+zddot*ddt;
            z=z_pre+zdot*ddt;
            z_pre=z;
            zdot_pre=zdot;

           xxd_.p(2) = zz+z;
           xxd_dot_(2)=0+zdot;
           xxd_ddot_(2)=0+zddot;

            // publisher    *************************************
            msg_temp.data[0]=zz;
            msg_temp.data[1]=zdot;
            msg_temp.data[2]=zddot;
            msg_temp.data[3]=xxd_.p(2);
            msg_temp.data[4]=F;
            msg_temp.data[5]=total_time_;

            publisher_.publish(msg_temp);

            // ********* 2. Inverse Kinematics *********
            // *** 2.0 Error Definition in Task Space ***
            eex_temp_ = diff(xx_, xxd_);
            eex_(0) = eex_temp_(0);
            eex_(1) = eex_temp_(1);
            eex_(2) = eex_temp_(2);
            eex_(3) = eex_temp_(3);
            eex_(4) = eex_temp_(4);
            eex_(5) = eex_temp_(5);

            // *** 2.1 computing Jacobian J(q) ***
            jjnt_to_jac_solver_->JntToJac(qq_, JJ_);

            xxdot_ = JJ_.data * qqdot_.data;
            eex_dot_ = xxd_dot_ - xxdot_;

            // *** 2.1 computing Jacobian J(q) ***
            jjnt_to_jac_solver_->JntToJac(qq_, JJ_);

            // *** 2.2 computing Jacobian transpose/inversion ***
            JJ_transpose_ = JJ_.data.transpose();
            JJ_inv_ = JJ_.data.inverse();

            if (tt < t_set)
            {
                qqd_.data = qqd_old_.data;
            }
            else
            {
                qqd_.data = qqd_old_.data + JJ_transpose_ * KK_regulation_ * eex_ * ddt;
                qqd_old_.data = qqd_.data;
            }

            // ********* 3. Motion Controller in Joint Space*********
            // *** 3.1 Error Definition in Joint Space ***
            ee_.data = qqd_.data - qq_.data;
            ee_dot_.data = qqd_dot_.data - qqdot_.data;
            ee_int_.data = qqd_.data - qq_.data; // (To do: e_int 업데이트 필요요 (Update required))

            // *** 3.2 Compute model(M,C,G) ***
            iid_solver_->JntToMass(qq_, MM_);
            iid_solver_->JntToCoriolis(qq_, qqdot_, CC_);
            iid_solver_->JntToGravity(qq_, GG_); // output은 머지? , id_solver는 어디에서?

            // *** 3.3 Apply Torque Command to Actuator ***

            aaux_d_.data =MM_.data*(JJ_inv_*(xxd_ddot_ + KKd_.data.cwiseProduct(eex_dot_)+KKp_.data.cwiseProduct(eex_)));

            //aaux_d_.data = MM_.data * (qqd_ddot_.data + KKp_.data.cwiseProduct(ee_.data) + KKd_.data.cwiseProduct(ee_dot_.data));
            ccomp_d_.data = CC_.data + GG_.data; //(n(q,q_dot))
            ttau_d_.data = aaux_d_.data + ccomp_d_.data;

            for (int i = 0; i < nn_joints_; i++)
            {
                jjoints_[i].setCommand(ttau_d_(i));
            }

        }else {

            qddot_.data = (qdot_.data - qdot_old_.data) / dt_;

            // error
            KDL::JntArray q_error;
            KDL::JntArray q_error_dot;
            KDL::JntArray ded;
            KDL::JntArray tde;
            KDL::JntArray s;

            q_error.data = Eigen::VectorXd::Zero(n_joints_);
            q_error_dot.data = Eigen::VectorXd::Zero(n_joints_);
            ded.data = Eigen::VectorXd::Zero(n_joints_);
            tde.data = Eigen::VectorXd::Zero(n_joints_);
            s.data = Eigen::VectorXd::Zero(n_joints_);

            double db = 0.001;

            q_error.data = q_cmd_.data - q_.data;
            q_error_dot.data = qdot_cmd_.data - qdot_.data;

            for(size_t i=0; i<n_joints_; i++)
            {
                s(i) = q_error_dot(i) + Ramda_(i)*q_error(i);

                if(db < Mbar_(i) && Mbar_(i) > Ramda_(i)/Alpha_(i))
                {
                    Mbar_dot_(i) = Alpha_(i)*s(i)*s(i) - Alpha_(i)*Omega_(i)*Mbar_(i);
                }

                if(Mbar_(i) < db)
                {
                    Mbar_(i) = db;
                }

                if(Mbar_(i) > Ramda_(i)/Alpha_(i))
                {
                    Mbar_(i) = Ramda_(i)/Alpha_(i) - db;
                }

                Mbar_(i) = Mbar_(i) + dt_*Mbar_dot_(i);
            }

            // torque command
            for(size_t i=0; i<n_joints_; i++)
            {
                ded(i) = qddot_cmd_(i) + 2.0 * Ramda_(i)*q_error(i) + Ramda_(i)*Ramda_(i)*q_error_dot(i);
                tde(i) = tau_cmd_old_(i) - Mbar_(i)*qddot_(i);
                tau_cmd_(i) = Mbar_(i)*ded(i) + tde(i);
            }

            for(size_t i=0; i<n_joints_; i++)
            {
                joints_[i].setCommand(tau_cmd_(i));
            }

        }

        tau_cmd_old_.data = tau_cmd_.data;
        qdot_old_.data = qdot_.data;

        KDL::Frame Xdes;

        fk_solver_->JntToCart(q_cmd_, Xdes);

        SaveData_[0] = total_time_;
        SaveData_[1] = 0.122;
        SaveData_[2] = Xdes.p(2);
        SaveData_[3] = Fd_;
        SaveData_[4] = f_cur_(1);
        SaveData_[5] = f_cur_buffer_;
        SaveData_[6] = B_buffer_;

        msg_SaveData_.data.clear();

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

        time_ = time_ + dt_;
        total_time_ = total_time_ + dt_;

    }

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

    void task_via()
    {
        time_ = 0.0;

        for (size_t i = 0; i < n_joints_; i++)
        {
            q_init_(i) = joints_[i].getPosition();
            q_cmd_(i) = q_cmd_end_(i);
            qdot_cmd_(i) = 0.0;
            qddot_cmd_(i) = 0.0;
        }
    }

    void task_init()
    {
        for (size_t i=0; i<n_joints_; i++)
        {
            q_cmd_(i) = 0.0;
            qdot_cmd_(i) = 0.0;
            qddot_cmd_(i) = 0.0;
        }

        q_cmd_end_.data = q_cmd_.data;
    }

    void task_ready()
    {
        for (size_t i=0; i<n_joints_; i++)
        {
            if (i == 2 || i == 4)
            {
                q_cmd_(i) = trajectory_generator_pos(q_init_(i), PI/2.0, 10.0);
                qdot_cmd_(i) = trajectory_generator_vel(q_init_(i), PI/2.0, 10.0);
                qddot_cmd_(i) = trajectory_generator_acc(q_init_(i), PI/2.0, 10.0);
            }
            else
            {
                q_cmd_(i) = q_init_(i);
                qdot_cmd_(i) = 0.0;
                qddot_cmd_(i) = 0.0;
            }
        }

        q_cmd_end_.data = q_cmd_.data;
    }

    void task_freespace()
    {
        KDL::Frame start;
        KDL::Twist target_vel;
        KDL::JntArray cart_cmd;

        cart_cmd.data = Eigen::VectorXd::Zero(3);

        fk_solver_->JntToCart(q_init_, start);

        for (size_t i=0; i<6; i++)
        {
            if(i == 2)
            {
                target_vel(i) = trajectory_generator_vel(start.p(i), 0.122, 10.0);
            }
            else
            {
                target_vel(i) = 0.0;
            }
        }

        ik_vel_solver_->CartToJnt(q_cmd_, target_vel, qdot_cmd_);

        for (size_t i=0; i<n_joints_; i++)
        {
            q_cmd_(i) = q_cmd_(i) + qdot_cmd_(i)*dt_;
            qddot_cmd_(i) = (qdot_cmd_(i) - qdot_cmd_old_(i))/dt_;
            qdot_cmd_old_(i) = qdot_cmd_(i);
        }

        q_cmd_end_.data = q_cmd_.data;
    }

    void task_contactspace()
    {
        KDL::Frame start;

        if (experiment_mode_ == 0 || experiment_mode_ == 1)
            Fd_ = Fd_temp_;
        else if (experiment_mode_ == 2)
            Fd_ = Fd_temp_ * 1 / 5 * sin(2 * PI * 0.2 * total_time_) + Fd_temp_;

        fk_solver_->JntToCart(q_init_, start);

        for (size_t i = 0; i < 6; i++)
        {
            if (i == 0)
            {
                Xc_dot_(i) = trajectory_generator_vel(start.p(i), 0.5, 20.0);
            }
            else if (i == 2)
            {
                Fe_ = f_cur_[1];
                PI_ = PI_old_ + dt_ * (Fd_old_ - Fe_old_) / B_;
                B_buffer_ = B_ + del_B_;
                Xc_ddot_(i) = 1/M_*((Fe_ - Fd_) - (B_ + del_B_)*Xc_dot_old_(i));
                Xc_dot_(i) = Xc_dot_old_(i) + Xc_ddot_(i)*dt_;
                del_B_ = B_ / Xc_dot_(i) * PI_;
                Xc_dot_old_(i) = Xc_dot_(i);
                Fd_old_ = Fd_;
                Fe_old_ = Fe_;
                PI_old_ = PI_;
            }
            else
            {
                Xc_ddot_(i) = 0.0;
                Xc_dot_(i) = 0.0;
            }
        }

        ik_vel_solver_->CartToJnt(q_cmd_, Xc_dot_, qdot_cmd_);

        for (size_t i=0; i<n_joints_; i++)
        {
            q_cmd_(i) = q_cmd_(i) + qdot_cmd_(i)*dt_;
            qddot_cmd_(i) = (qdot_cmd_(i) - qdot_cmd_old_(i))/dt_;
            qdot_cmd_old_(i) = qdot_cmd_(i);
        }

        q_cmd_end_.data = q_cmd_.data;
    }

    void task_homming()
    {
        for (size_t i = 0; i < n_joints_; i++)
        {
            q_cmd_(i) = trajectory_generator_pos(q_init_(i), 0.0, 10.0);
            qdot_cmd_(i) = trajectory_generator_vel(q_init_(i), 0.0, 10.0);
            qddot_cmd_(i) = trajectory_generator_acc(q_init_(i), 0.0, 10.0);
        }

        q_cmd_end_.data = q_cmd_.data;
    }

    double trajectory_generator_pos(double dStart, double dEnd, double dDuration)
    {
        double dA0 = dStart;
        double dA3 = (20.0*dEnd - 20.0*dStart) / (2.0*dDuration*dDuration*dDuration);
        double dA4 = (30.0*dStart - 30*dEnd) / (2.0*dDuration*dDuration*dDuration*dDuration);
        double dA5 = (12.0*dEnd - 12.0*dStart) / (2.0*dDuration*dDuration*dDuration*dDuration*dDuration);

        return dA0 + dA3*time_*time_*time_ + dA4*time_*time_*time_*time_ + dA5*time_*time_*time_*time_*time_;
    }

    double trajectory_generator_vel(double dStart, double dEnd, double dDuration)
    {
        double dA0 = dStart;
        double dA3 = (20.0*dEnd - 20.0*dStart) / (2.0*dDuration*dDuration*dDuration);
        double dA4 = (30.0*dStart - 30*dEnd) / (2.0*dDuration*dDuration*dDuration*dDuration);
        double dA5 = (12.0*dEnd - 12.0*dStart) / (2.0*dDuration*dDuration*dDuration*dDuration*dDuration);

        return 3.0*dA3*time_*time_ + 4.0*dA4*time_*time_*time_ + 5.0*dA5*time_*time_*time_*time_;
    }

    double trajectory_generator_acc(double dStart, double dEnd, double dDuration)
    {
        double dA0 = dStart;
        double dA3 = (20.0*dEnd - 20.0*dStart) / (2.0*dDuration*dDuration*dDuration);
        double dA4 = (30.0*dStart - 30*dEnd) / (2.0*dDuration*dDuration*dDuration*dDuration);
        double dA5 = (12.0*dEnd - 12.0*dStart) / (2.0*dDuration*dDuration*dDuration*dDuration*dDuration);

        return 6.0*dA3*time_ + 12.0*dA4*time_*time_ + 20.0*dA5*time_*time_*time_;
    }

    double first_order_lowpass_filter()
    {
        filt_ = (tau_ * filt_old_ + dt_*f_cur_buffer_)/(tau_ + dt_);
        filt_old_ = filt_;

        return filt_;
    }

private:
    // joint handles
    unsigned int n_joints_;
    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_;
    boost::scoped_ptr<KDL::ChainDynParam> id_solver_;
    boost::scoped_ptr<KDL::ChainFkSolverPos> fk_solver_;
    boost::scoped_ptr<KDL::ChainIkSolverVel_pinv> ik_vel_solver_;
    boost::scoped_ptr<KDL::ChainIkSolverPos_NR_JL> ik_pos_solver_;
    KDL::JntArray G_;
    KDL::Vector gravity_;

    // tdc gain
    KDL::JntArray Mbar_, Mbar_dot_, Ramda_, Alpha_, Omega_;

    // cmd, state
    KDL::JntArray q_init_;
    KDL::JntArray tau_cmd_, tau_cmd_old_;
    KDL::JntArray q_cmd_, q_cmd_old_, qdot_cmd_, qdot_cmd_old_, qddot_cmd_;
    KDL::JntArray q_cmd_end_;
    KDL::JntArray q_cmd_sp_;
    KDL::JntArray q_, qdot_, qdot_old_, qddot_;
    KDL::Wrench f_cur_;
    double f_cur_buffer_;

    KDL::Twist Xc_dot_, Xc_dot_old_, Xc_ddot_;

    double Xr_dot_, Xe_dot_;
    double Xe_ddot_;

    double Fd_, Fd_temp_, Fd_old_, Fe_, Fe_old_;
    double M_, B_, del_B_, B_buffer_;
    double PI_, PI_old_;

    double dt_;
    double time_;
    double total_time_;

    double filt_old_;
    double filt_;
    double tau_;

    int experiment_mode_;

    // topic
    ros::Subscriber sub_q_cmd_;
    ros::Subscriber sub_forcetorque_sensor_;

    double SaveData_[SaveDataMax];

    ros::Publisher pub_SaveData_;

    std_msgs::Float64MultiArray msg_SaveData_;

    // new from computed torque controller:

    // others
    double tt;
    int cctr_obj_;
    int iik_mode_;
    int eevent;

    //Joint handles
    unsigned int nn_joints_;                               // joint 숫자
    std::vector<std::string> jjoint_names_;                // joint name ??
    std::vector<hardware_interface::JointHandle> jjoints_; // ??
    std::vector<urdf::JointConstSharedPtr> jjoint_urdfs_;  // ??

    // kdl
    KDL::Tree kkdl_tree_;   // tree?
    KDL::Chain kkdl_chain_; // chain?

    // kdl M,C,G
    KDL::JntSpaceInertiaMatrix MM_; // intertia matrix
    KDL::JntArray CC_;              // coriolis
    KDL::JntArray GG_;              // gravity torque vector
    KDL::Vector ggravity_;

    // kdl and Eigen Jacobian
    KDL::Jacobian JJ_;
    // KDL::Jacobian J_inv_;
    // Eigen::Matrix<double, num_taskspace, num_taskspace> J_inv_;
    Eigen::MatrixXd JJ_inv_;
    Eigen::Matrix<double, num_taskspace, num_taskspace> JJ_transpose_;

    // kdl solver
    boost::scoped_ptr<KDL::ChainFkSolverPos_recursive> ffk_pos_solver_; //Solver to compute the forward kinematics (position)
    // boost::scoped_ptr<KDL::ChainFkSolverVel_recursive> fk_vel_solver_; //Solver to compute the forward kinematics (velocity)
    boost::scoped_ptr<KDL::ChainJntToJacSolver> jjnt_to_jac_solver_; //Solver to compute the jacobian
    boost::scoped_ptr<KDL::ChainDynParam> iid_solver_;               // Solver To compute the inverse dynamics

    // Joint Space State
    KDL::JntArray qqd_, qqd_dot_, qqd_ddot_;
    KDL::JntArray qqd_old_;
    KDL::JntArray qq_, qqdot_;
    KDL::JntArray ee_, ee_dot_, ee_int_;

    // Task Space State
    // ver. 01
    KDL::Frame xxd_; // x.p: frame position(3x1), x.m: frame orientation (3x3)
    KDL::Frame xx_;
    KDL::Twist eex_temp_;

    // KDL::Twist xd_dot_, xd_ddot_;
    Eigen::Matrix<double, num_taskspace, 1> eex_;
    Eigen::Matrix<double, num_taskspace, 1> xxd_dot_, xxd_ddot_;
    Eigen::Matrix<double, num_taskspace, 1> xxdot_;
    Eigen::Matrix<double, num_taskspace, 1> eex_dot_, eex_int_;

    // ver. 02
    // Eigen::Matrix<double, num_taskspace, 1> xd_, xd_dot_, xd_ddot_;
    // Eigen::Matrix<double, num_taskspace, 1> x_, xdot_;
    // KDL::Frame x_temp_;
    // Eigen::Matrix<double, num_taskspace, 1> ex_, ex_dot_, ex_int_;

    // Input
    KDL::JntArray xx_cmd_;

    // Torque
    KDL::JntArray aaux_d_;
    KDL::JntArray ccomp_d_;
    KDL::JntArray ttau_d_;

    // gains
    KDL::JntArray KKp_, KKi_, KKd_;
    double KK_regulation_, KK_tracking_;

    // save the data
    double SSaveData_[SaveDataMax];

    // ros subscriber
    ros::Subscriber ssub_x_cmd_;

    // ros publisher
    ros::Publisher ppub_qd_, ppub_q_, ppub_e_;
    ros::Publisher ppub_xd_, ppub_x_, ppub_ex_;
    ros::Publisher ppub_SaveData_;

    // ros message
    std_msgs::Float64MultiArray mmsg_qd_, mmsg_q_, mmsg_e_;
    std_msgs::Float64MultiArray mmsg_xd_, mmsg_x_, mmsg_ex_;
    std_msgs::Float64MultiArray mmsg_SaveData_;

    // magni
    int flag = 0;
    double xx;
    double zz;

    // admittance params
    double z_pre;
    double zdot_pre;
    double zddot_pre;
    double z;
    double zdot;
    double zddot;
    double K_damp;
    double K_stiff;
    double M_ad;
    int window;

    ros::Subscriber k_stiff_damp_sub;

    std_msgs::Float64MultiArray msg_temp;
    ros::Publisher publisher_;

};
}

PLUGINLIB_EXPORT_CLASS(arm_controllers::AdmittanceController, controller_interface::ControllerBase)


#include "ros/ros.h"
#include <tf/tf.h>
#include <tf/transform_listener.h>
#include <math.h>
#include <sstream>
#include <string>
#include <iostream>
#include <cstdio>
#include <ctime>
#include <unistd.h>
#include <cmath>
#include <stdio.h>
#include <stdlib.h>
#include <tbb/atomic.h>
#include <tf/transform_broadcaster.h>
#include "std_msgs/Int32.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "sepanta_msgs/omnidata.h"
#include <dynamixel_msgs/MotorStateList.h>
#include <dynamixel_msgs/JointState.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PolygonStamped.h>
#include <move_base_msgs/MoveBaseActionGoal.h>
#include <dynamixel_controllers/SetComplianceSlope.h>
#include <dynamixel_controllers/SetCompliancePunch.h>
#include <boost/thread.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/lexical_cast.hpp>
#include "std_msgs/Int32.h"
#include "std_msgs/String.h"
#include "std_msgs/Bool.h"
#include "geometry_msgs/Twist.h"
#include <sensor_msgs/LaserScan.h>
#include <termios.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_datatypes.h>
#include <sepanta_msgs/command.h>
#include <sepanta_msgs/omnidata.h>
#include <sepanta_msgs/sepantaAction.h> //movex movey turngl turngllocal actions
#include <sepanta_msgs/slamactionAction.h> //slam action
#include <ros/package.h>
#include <fstream>
#include <iostream>
#include <stdlib.h>
#include <sstream>
#include <vector>
#include <fstream>
#include <nav_core/base_local_planner.h>
#include <nav_core/base_global_planner.h>
#include <nav_core/recovery_behavior.h>
#include <geometry_msgs/PoseStamped.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <costmap_2d/costmap_2d.h>
#include <nav_msgs/GetPlan.h>
#include <std_srvs/Empty.h>
#include <tf/transform_datatypes.h>
#include <geometry_msgs/PoseStamped.h>
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <sepanta_msgs/Objects.h>
#include <sepanta_msgs/Object.h>
#include <k2_client/BodyArray.h>
///////////////////////////////////////////////////////////
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/highgui/highgui_c.h"
#include <opencv2/core/core.hpp>
#include "opencv/cv.h"
#include "opencv2/calib3d/calib3d.hpp"
//********************************************** cv_bridge
#include <cv_bridge/cv_bridge.h>
#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>

#include <sensor_msgs/Image.h>
#include <sepanta_msgs/Objects.h>
#include <image_transport/image_transport.h>

#include <boost/thread/mutex.hpp>

#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <sepanta_msgs/LookForObjectsAction.h>

#include <actionlib/client/simple_action_client.h>
#include <actionlib/client/terminal_state.h>
#include <sepanta_msgs/MasterAction.h>
#include <sepanta_msgs/led.h>

using std::string;
using std::exception;
using std::cout;
using std::cerr;
using std::endl;

using namespace std;
using namespace boost;
using namespace ros;

//=============================================================
#define waypoint_1 "roomcenter"
#define waypoint_2 "bedroom"
#define waypoint_3 "shelf"
#define waypoint_4 "kitchen"
#define ask_for_recover

enum _logic_state { idle, wait_for_door, on_waypoint1 , on_waypoint2 , on_waypoint3 , on_waypoint4 , go_to_waypoint1 , go_to_waypoint2 , go_to_waypoint3 , go_to_waypoint4 , recover_door , recover_obstacle , recover_human };
enum _navigation_state { ready , run , cancel , error };
//=============================================================

std::string coutcolor0 = "\033[0;0m";
std::string coutcolor_red = "\033[0;31m";
std::string coutcolor_green = "\033[0;32m";
std::string coutcolor_blue = "\033[0;34m";
std::string coutcolor_magenta = "\033[0;35m";
std::string coutcolor_brown = "\033[0;33m";

int Function_state = 0;
bool Function1_result = false;
bool Function2_result = false; //dummy
bool Function3_result = false;
int question_counter = 0;
int door_distance = 0;
bool isdooropened_kinect2 = true;

bool App_exit = false;
ros::Publisher pub_tts;
ros::Publisher pub_spr;
ros::Publisher pub_led;
bool say_enable = false;

_logic_state logic_state = idle;
bool isdooropened = false;
bool ishumanthere = false;
bool isspeechready = false;
bool isttsready = true;

string sayMessageId;

string speech_last_command = "";
string temp_speech_last_command = "";
ros::ServiceClient client_navigation;
ros::ServiceClient say_service;

actionlib::SimpleActionClient<sepanta_msgs::MasterAction> * ac_navigation;

void change_led(int r,int g,int b)
{
  sepanta_msgs::led _msg;

  if( r != 0 || g != 0 || b != 0)
  {
     _msg.enable = true;
     _msg.colorR = r;
     _msg.colorG = g;
     _msg.colorB = b;
  }
  else
  {
    _msg.enable = false;
  }

  pub_led.publish(_msg);
}

string last_print_message = "";
void print_color(string msg,string color,bool once = false)
{
  if ( once == false )
  {
    if ( color == "red" )
    {
      std::cout<<coutcolor_red<<msg<<coutcolor0<<std::endl;
    }
    if ( color == "green" )
    {
      std::cout<<coutcolor_green<<msg<<coutcolor0<<std::endl;
    }
    if ( color == "blue" )
    {
      std::cout<<coutcolor_blue<<msg<<coutcolor0<<std::endl;
    }
    if ( color == "brown" )
    {
      std::cout<<coutcolor_brown<<msg<<coutcolor0<<std::endl;
    }
    if ( color == "coutcolor_magenta" )
    {
      std::cout<<coutcolor_magenta<<msg<<coutcolor0<<std::endl;
    }
  }
  else
  {
       if ( last_print_message != msg)
       {
           speech_last_command = msg;

           if ( color == "red" )
          {
            std::cout<<coutcolor_red<<speech_last_command<<coutcolor0<<std::endl;
          }
          if ( color == "green" )
          {
            std::cout<<coutcolor_green<<speech_last_command<<coutcolor0<<std::endl;
          }
          if ( color == "blue" )
          {
            std::cout<<coutcolor_blue<<speech_last_command<<coutcolor0<<std::endl;
          }
          if ( color == "brown" )
          {
            std::cout<<coutcolor_brown<<speech_last_command<<coutcolor0<<std::endl;
          }
          if ( color == "coutcolor_magenta" )
          {
            std::cout<<coutcolor_magenta<<speech_last_command<<coutcolor0<<std::endl;
          }

       }
  }
}

void sepanta_wait(int ms)
{
  boost::this_thread::sleep(boost::posix_time::milliseconds(ms));
}

string navigation_go_to(string location)
{
  sepanta_msgs::MasterGoal goal;
  goal.action = "exe";
  goal.id = location;

  ac_navigation->sendGoal(goal);

  //wait for the action to return
  bool finished_before_timeout = ac_navigation->waitForResult(ros::Duration(1000));

  if (finished_before_timeout)
  {
    actionlib::SimpleClientGoalState state = ac_navigation->getState();
    ROS_INFO("Action finished: %s",state.toString().c_str());
    sepanta_msgs::MasterResult::ConstPtr _res = ac_navigation->getResult();
    ROS_INFO("Action result : %s",_res->result.c_str());

    return _res->result.c_str();

  }
  else
  {
     ac_navigation->cancelGoal();
     ROS_INFO("Action did not finish before the time out.");

      return "time_out";
  }

}

void navigation_cancel()
{
   ac_navigation->cancelGoal();
}

string temp_say_message = "";

void say_message(string data,bool once = false)
{
    if  ( once )
    {
      if ( temp_say_message == data ) return;
      temp_say_message = data;
    }
    if ( say_enable == false ) return;
  	isttsready = false;
    sepanta_msgs::command _msg;
   _msg.request.command = data;
    say_service.call(_msg);
    sayMessageId = _msg.response.result;
    while(!isttsready)
    {
    	sepanta_wait(1000);
    }
}

void chatterCallback_kinect2_body(const k2_client::BodyArray::ConstPtr &msg)
{
   bool detect = false;

   for ( int i = 0 ; i < msg->bodies.size() ; i++)
   {
       if ( msg->bodies.at(i).isTracked)
       {
          int _z =  msg->bodies.at(i).jointPositions.at(11).position.z * 100;
          int _x =  msg->bodies.at(i).jointPositions.at(11).position.x * 100;

          stringstream convert; // stringstream used for the conversion
          convert << _z;

          string z = convert.str();

          if ( _z < 200 && _z > 10 )
          {
             if (  abs(_x) < 50 )
             {
                 ishumanthere = true;
                 detect = true;
             }
          }

          print_color(z,"red",false);
       }

   }

   if ( detect == false)
   {
      ishumanthere = false;
   }
}

void chatterCallback_door(const std_msgs::Bool::ConstPtr &msg)
{
   isdooropened = msg->data;
}

void chatterCallback_speech(const std_msgs::String::ConstPtr &msg)
{
   cout<<coutcolor_brown<<"Speech Get : "<<msg->data<<coutcolor0<<endl;
   if ( temp_speech_last_command != msg->data )
   {
      temp_speech_last_command = msg->data;
      speech_last_command = temp_speech_last_command;
      isspeechready = true;
   }
}

void chatterCallback_ttsfb(const std_msgs::String::ConstPtr &msg)
{
	if(!isttsready && msg->data==sayMessageId)
	{
		isttsready = true;
	}
}

void send_feedback_to_speech(string cmd)
{
   std_msgs::String _msg;
   _msg.data = cmd;
   pub_spr.publish(_msg);
}

void Function_1()
{
   // if ( Function_state == 0 )
   // {
   //    say_message("I am going to kitchen and i should bring a coca for you!");
   //    Function_state = 1;
   // }
   // else if ( Function_state == 1)
   // {
   //    navigation_go_to("kitchen");
   //    Function_state = 2;
   // }
   // else if ( Function_state == 2)
   // {
   //      cout<<coutcolor_blue<<"Function [1] STATE [2] : wait for navigation to kitchen"<<coutcolor0<<endl;
	  //   	Function_state = 3;
   // }
   // else if ( Function_state == 3)
   // {
   //     Function1_result = false;
   //     cout<<coutcolor_blue<<"Function [1] STATE [3] : Send Start for object"<<coutcolor0<<endl;
   //     Function_state = 4;
   // }
   // else if ( Function_state == 4)
   // {
   // 	   cout<<coutcolor_blue<<"Function [1] STATE [4] : wait for object recognition"<<coutcolor0<<endl;
   // 	   Function_state = 5;
   // }
   // else if ( Function_state == 5)
   // {
   //    Function1_result = process_object("coca");
   //    if ( Function1_result )
   //    {
   //      say_message("well done ! , i found the coca !");
   //      say_message("i wonder, if i had my arms , could i pick up the coca? Money does not guarantee hapiness. But no money, no arms! ");
   //    }
   //    else
   //    {
   //      say_message("I cant find the coca.");
   //      say_message("it does not matter. I could not grab it even if I found it. Money does not guarantee hapiness. But no money, no arms! ");
   //    }

   //    Function_state = 6;
   // }
   // else if ( Function_state == 6)
   // {
   //    say_message("i am goint to room center to report my perception");
   //    navigation_go_to("roomcenter");
   //    Function_state = 7;
   // }
   // else if ( Function_state == 7)
   // {
   // 	   cout<<coutcolor_blue<<"Function [1] STATE [7] : wait for navigation to room center"<<coutcolor0<<endl;
	  //    Function_state = 8;
   // }
   // else if ( Function_state == 8 )
   // {

   //    if ( Function1_result )
   //    {
   //         say_message("well done ! , i found the coca !");
   //         say_message("But if you expect me to bring it for you, prepare a set of arms for me!");
   //    }
   //    else
   //    {
   //         say_message("I cant find the coca.");
   //         say_message("it does not matter. I could not grab it even if I found it.");
   //    }
   // 	   //========================================
   // 	   say_message("operation Done");

   // 	   Function_state = 0;
   // 	   logic_state = 3;
   // }
}

void Function_2()
{
   // cout<<"Header :"<<Function_state<<endl;
   // if ( Function_state == 0 )
   // {
   //   question_counter = 0;
   // 	 say_message("It is my pleasure to answer your questions!");
   // 	 Function_state = 1;
   // }
   // else if ( Function_state == 1)
   // {
   // 	  say_message("ask");
   //    send_feedback_to_speech("qstart");
   //    Function_state = 2;
   // }
   // else if ( Function_state == 2)
   // {
   //    if ( isspeechready )
   //    {
   //        isspeechready = false;
   //        if ( speech_last_command == "qready")
   //        {
   //          //say_message("ask");
   //          Function_state = 3;
   //        }
   //        else
   //        {

   //           send_feedback_to_speech("qstart");
   //        }
   //    }
   // }
   // else if ( Function_state == 3)
   // {
   //          //Ready
   //          if ( isspeechready )
   //          {
   //             isspeechready = false;
   //             if ( speech_last_command != "0")
   //             {
   //               say_message(speech_last_command);
   //               if ( question_counter < 4)
   //               {
   //               question_counter++;
   //               Function_state = 1;
   //               }
   //               else
   //               {
   //                  say_message("Finished");
   //                  Function_state = 4;
   //               }
   //             }
   //             else
   //             {
   //                 Function_state = 1;
   //             }
   //          }
   //          else
   //          {
   //                cout<<coutcolor_blue<<"Function [2] STATE [3] : wait for user question"<<coutcolor0<<endl;
   //          }
   // }
   // else if ( Function_state == 4)
   // {
   //     Function_state = 0;
   //     logic_state = 3;
   // }
}

void Function_3()
{
     // if ( Function_state == 0 )
     // {
     //   string cmd = "I am going to find the " + desire_object_name + " for you";
     //   say_message(cmd);
     //   Function_state = 1;
     // }
     // else if ( Function_state == 1)
     // {
     //   navigation_go_to("bedroom");
     //   Function_state = 2;
     // }
     // else if ( Function_state == 2)
     // {
     //      cout<<coutcolor_blue<<"Function [3] STATE [2] : wait for navigation to bedroom"<<coutcolor0<<endl;
  	  //   	Function_state = 3;
     // }
     // else if ( Function_state == 3)
     // {
     //     Function3_result = false;
     // 	   Function_state = 4;
     // }
     // else if ( Function_state == 4)
     // {
     //     cout<<coutcolor_blue<<"Function [3] STATE [4] : wait for object recognition"<<coutcolor0<<endl;
     // 	   Function_state = 5;
     // }
     // else if ( Function_state == 5)
     // {
     //    bool result = process_object(desire_object_name);

     //    if ( result )
     //    {
     //      //we find it
     //      string cmd = "I Find the " + desire_object_name + " for you";
     //      say_message(cmd);

     //      cmd = "i wonder, if i had my arms could i pick up the  " +  desire_object_name + " ? Oh money ! money is the problem";
     //      say_message(cmd);

     //      say_message("i am going to the room center");
     //      navigation_go_to("roomcenter");

     //      Function_state = 10;

     //    }
     //    else
     //    {
     //      //we cant find it
     //      string cmd = "I Could not find the " + desire_object_name + " in bedroom ";
     //      say_message(cmd);

     //      say_message("it is better to check the shelf , maybe i will find it there ");
     //      navigation_go_to("shelf");

     //      Function_state = 6;
     //    }
     // }
     // else if ( Function_state == 6)
     // {
     //      cout<<coutcolor_blue<<"Function [3] STATE [6] : wait for navigation to shelf"<<coutcolor0<<endl;
  	  //   	Function_state = 7;
     // }
     // else if ( Function_state == 7)
     // {
     //    Function3_result = false;
     // 	  Function_state = 8;
     // }
     // else if ( Function_state == 8)
     // {
     //     cout<<coutcolor_blue<<"Function [3] STATE [8] : wait for object recognition"<<coutcolor0<<endl;
     // 	   Function_state = 9;
     // }
     // else if ( Function_state == 9)
     // {
     //   bool result = process_object(desire_object_name);
   	 //   string cmd = "";
     //   if ( result )
     //    {
     //      //we find it
     //       cmd = "I Find the " + desire_object_name + " for you";
     //       say_message(cmd);
     //       cmd = "I have the  " +  desire_object_name + ". But wait! How on earth can I pick it up when I have no arms!?";
     //       say_message(cmd);
     //    }
     //    else
     //    {
     //      //we cant find it
     //     cmd = "I Could not find the " + desire_object_name + " on the shelf ";
     //     say_message(cmd);
     //     cmd = "i want to have 2 arms like my other friends and humans ! Oh money ! money is the problem";
     //     say_message(cmd);

     //    }

     //      say_message("i am going to the room center");
     //      navigation_go_to("roomcenter");
     //      Function_state = 10;
     //   }
     //   else if ( Function_state == 10)
     //   {
     //   	    cout<<coutcolor_blue<<"Function [3] STATE [10] : wait for navigation to room center"<<coutcolor0<<endl;
    	//     	Function_state = 11;

     //   }
     //   else if ( Function_state == 11 )
     //   {
     //   	   //report object recognition status
     //   	   //========================================
     //   	   say_message("operation Done");
     //   	   Function_state = 0;
     //   	   logic_state = 3;
     //   }
}

void process_speech_command(string message)
{
   // isspeechready = false;
   // speech_last_command = "";

   // if ( logic_state == 4 )
   // {
	  //  	if ( message == "ready")
	  //  	{
		 //   	logic_state = 5;
		 //   	cout<<coutcolor_green<<"Speech is ready !"<<coutcolor0<<endl;
	  //   }
	  //   else if ( message == "#error#" )
	  //   {
	  //   	logic_state = 3;
	  //   	cout<<coutcolor_red<<"Speech has error :"<< message <<coutcolor0<<endl;
	  //   }
   // }
   // else if ( logic_state == 5 )
   // {
   //    //6 kitchen
   //    //7 questions
   //    //8 object
   //    //=================================================
   //     // cout<<coutcolor_green<<"SPEECH GET PROCESS : "<<message<<coutcolor0<<endl;
   // 	   // if ( message == "1" ){logic_state = 6;}else
   // 	   // if ( message == "2" ){logic_state = 7;}else
   // 	   // if ( message == "3" ){logic_state = 8; desire_object_name = "soda";}else
   // 	   // if ( message == "4" ){logic_state = 8; desire_object_name = "coffee";}else
   // 	   // if ( message == "5" ){logic_state = 9;}

   // 	   // else
   // 	   // {
   //     // cout<<coutcolor_red<<"Invalid Speech Command for logic state 5 : "<< message <<coutcolor0<<endl;
   //     // logic_state = 3;
   // 	   // }
   // }
}

string desire_location = "";
_navigation_state navigation_state = ready;

void navigation_thread()
{
   sepanta_wait(1000);
   while ( App_exit == false && ros::ok())
   {
          if ( navigation_state == ready )
          {
              //wait for state
          }
          else if ( navigation_state == run)
          {
              navigation_go_to(desire_location);
              navigation_state = ready;
          }
          else if (navigation_state == cancel)
          {
             navigation_cancel();
             navigation_state = error;
          }
          else if ( navigation_state == error)
          {
            //there was a problem with navigation
          }

          sepanta_wait(100);
   }
}

_logic_state temp_logic_state = idle;

void check_for_obstacles()
{
                 if ( isdooropened == false && isdooropened_kinect2 == false)
                 {
                    change_led(255,0,0); //door
                    temp_logic_state = logic_state;
                    logic_state = recover_door;
                    navigation_state = cancel;
                    say_message("the door is closed");
                 }
                 else if ( isdooropened == false && isdooropened_kinect2 == true )
                 {
                    change_led(200,50,0); //object
                    temp_logic_state = logic_state;
                    logic_state = recover_obstacle;
                    navigation_state = cancel;
                    say_message("there is an object on my way");
                 }
                 else if ( isdooropened == true && isdooropened_kinect2 == true )
                 {
                    //change_led(0,255,0); //no obstacle
                 }
                 //===================================================
                 if ( ishumanthere )
                 {
                    change_led(255,0,255);
                    temp_logic_state = logic_state;
                    logic_state = recover_human;
                    navigation_state = cancel;
                    say_message("There is a human on my way");
                    say_message("Please move away!");
                 }
}

void logic_thread()
{
    sepanta_wait(1000);
    say_message("Sepanta Stage 1. Navigation test started");

    while(ros::ok() && !App_exit )
    {
         sepanta_wait(50);

         if ( logic_state == idle )
         {
           print_color("idle","blue",false);
           change_led(0,0,0);
           isspeechready = false;
           logic_state = wait_for_door;
         }
         else if ( logic_state == wait_for_door )
         {
            if ( isdooropened == false )
            {
                print_color("wait for door","brown",true);
                change_led(0,0,250);
            }
            else
            {
                print_color("the door is opened","green",false);
                say_message("The door is opened!");
                change_led(0,250,0);
                sepanta_wait(1000);
                change_led(0,0,0);
                desire_location = waypoint_1;
                navigation_state = run;
                logic_state = go_to_waypoint1;
            }
         }
         else
         if ( logic_state == go_to_waypoint1 )
         {
            print_color("STATE : wait for navigation to reach waypoint one","green",true);

            if ( navigation_state == ready )
            {
               logic_state = on_waypoint1;
            }
            else
            {
               check_for_obstacles();
            }

         }
         else if ( logic_state == on_waypoint1)
         {
          say_message("i reached waypoint one");

          desire_location = waypoint_2;
          navigation_state = run;
          logic_state = go_to_waypoint2;

         }
         else if ( logic_state == go_to_waypoint2)
         {
            print_color("STATE : wait for navigation to reach waypoint two","green",true);

            if ( navigation_state == ready )
            {
               logic_state = on_waypoint2;
            }
            else
            {
               check_for_obstacles();
            }
         }
         else if ( logic_state == on_waypoint2 )
         {
          say_message("i reached waypoint two");

          desire_location = waypoint_3;
          navigation_state = run;
          logic_state = go_to_waypoint3;
         }
         else if ( logic_state == go_to_waypoint3)
         {
            print_color("STATE : wait for navigation to reach waypoint three","green",true);

            if ( navigation_state == ready )
            {
               logic_state = on_waypoint3;
            }
            else
            {
               check_for_obstacles();
            }
         }
         else if ( logic_state == on_waypoint3 )
         {
          say_message("i reached waypoint three");
          say_message("i am ready to follow someone");
          //comming soon
         }
         else if ( logic_state == recover_door )
         {
             navigation_state = run;
             logic_state = temp_logic_state;
         }
         else if ( logic_state == recover_human )
         {
            if ( ishumanthere == false )
            {
            navigation_state = run;
            logic_state = temp_logic_state;
            }
         }
         else if ( logic_state == recover_obstacle )
         {

            #ifdef ask_for_recover

            say_message("Please move the object a way",true);

            if ( isdooropened )
            {
              say_message("Thanks for your copration");
              navigation_state == run;
              logic_state = temp_logic_state;
            }

            #else
             navigation_state = run;
             logic_state = temp_logic_state;
            #endif
         }

    }

     print_color("scenario logic terminated","red");
}

void chatterCallback_doorDistance(const std_msgs::Int32::ConstPtr& msg)
{
   door_distance = msg->data;
   if ( door_distance < 80 && door_distance > 10 )
   {
     isdooropened_kinect2 = false;
   }
   else
   {
     isdooropened_kinect2 = true;
   }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "scenario1");
    ros::Time::init();

    print_color("stage 1 , navigation test started","green");

    ros::NodeHandle node_handle;
    ros::Subscriber sub_handles[5];
    //=========================================================================================
    pub_tts = node_handle.advertise<std_msgs::String>("/texttospeech/message", 10);
    sub_handles[0] = node_handle.subscribe("lowerbodycore/isdooropened", 10, chatterCallback_door);
    sub_handles[1] = node_handle.subscribe("/speechRec/cmd_spch", 10, chatterCallback_speech);
    sub_handles[2] = node_handle.subscribe("/texttospeech/queue", 10, chatterCallback_ttsfb);
    sub_handles[3] = node_handle.subscribe("kinect2/bodyArray",10,chatterCallback_kinect2_body);
    sub_handles[4] = node_handle.subscribe("/doorDistance",10,chatterCallback_doorDistance);

    pub_spr = node_handle.advertise<std_msgs::String>("/speechRec/feedback_spch", 10);
    client_navigation = node_handle.serviceClient<sepanta_msgs::command>("sepantamovebase/command");
    say_service = node_handle.serviceClient<sepanta_msgs::command>("texttospeech/say");
    pub_led = node_handle.advertise<sepanta_msgs::led>("/lowerbodycore/led", 10);

    //==========================================================================================
    ros::Rate loop_rate(20);
    //action #1 SepantaNavigation
    ac_navigation = new actionlib::SimpleActionClient<sepanta_msgs::MasterAction>("SepantaMoveBaseAction", true);
    ROS_INFO("Waiting for action server to start [SepantaNavigation].");
    ac_navigation->waitForServer();
    ROS_INFO("SepantaNavigation Action Server Started ready");

    boost::thread _thread_Logic(&logic_thread);
    boost::thread _thread_Navigation(&navigation_thread);

    while (ros::ok() && App_exit == false)
    {
        ros::spinOnce();
        loop_rate.sleep();
    }

    _thread_Logic.interrupt();
    _thread_Logic.join();

    _thread_Navigation.interrupt();
    _thread_Navigation.join();

    App_exit = true;
    return 0;
}


#include <njson/json.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include "tachikoma.h"
#include "defs.h"
#include <signal.h>
#include <SDL/SDL.h>
#include <armadillo>

#define FPS 15

using namespace std;
using namespace arma;
using json = nlohmann::json;

static bool stopsig;

static void stopsignal(int) {
  stopsig = true;
}

int main(int argc, char *argv[]) {
  signal(SIGINT, stopsignal);

  // create a window to show the results of the calibration
  SDL_Init(SDL_INIT_EVERYTHING);
  SDL_Surface *screen;
  screen = SDL_SetVideoMode(640, 480, 32, SDL_SWSURFACE);

  // try to connect to the tachikoma's arduinos
  // NOTE: this will try to do ALL-IN-ONE Calibration
  Tachikoma *tachikoma;
  tachikoma = new Tachikoma();
  if (!tachikoma->connected()) {
    printf("[TACHI CALIBRATE] Not connected to anything, disconnecting...\n");
    tachikoma->disconnect();
    return 1;
  }

  // create space to store the position information
  mat leg_sensors;
  mat leg_feedback;

  uint32_t start;
  mat min_sensors;
  mat max_sensors;
  bool minset = false;
  bool maxset = false;

  // start reading events
  while (!stopsig) {
    SDL_Event event;
    start = SDL_GetTicks();

    while (SDL_PollEvent(&event)) {
      switch (event.type) {
        case SDL_QUIT:
          stopsig = true;
          break;
        default:
          break;
      }
    }
    if (stopsig) {
      continue;
    }

    // grab sensor readings
    tachikoma->recv(leg_sensors, leg_feedback);
    if (!minset) {
      minset = true;
      min_sensors = leg_sensors;
    } else {
      min_sensors += (leg_sensors - min_sensors) % (leg_sensors < min_sensors || min_sensors == 0);
    }
    if (!maxset) {
      maxset = true;
      max_sensors = leg_sensors;
    } else {
      max_sensors += (leg_sensors - max_sensors) % (leg_sensors > max_sensors || leg_sensors == 0);
    }

    cout << "[TACHI CALIBRATE] New data: \n";
    cout << leg_sensors.t() << endl;
    cout << min_sensors.t() << endl;
    cout << max_sensors.t() << endl;

    // SDL render
    SDL_Flip(screen);
    if (1000 / FPS > SDL_GetTicks() - start) {
      SDL_Delay(1000 / FPS - (SDL_GetTicks() - start));
    }
  }

  // convert to string and save to file
  json potvals = json({});
  vector<string> legnames = { "ul", "ur", "dl", "dr" };
  vector<int> legids = { UL, UR, DL, DR };
  vector<string> jointnames = { "waist", "thigh", "knee" };
  vector<int> jointids = { WAIST, THIGH, KNEE };
  for (int i = 0; i < NUM_LEGS; i++) {
    for (int j = 0; j < NUM_JOINTS; j++) {
      string legname = legnames[i];
      int legid = legids[i];
      string jointname = jointnames[j];
      int jointid = jointids[j];
      potvals[legname][jointname]["min"] = min_sensors(legid, jointid);
      potvals[legname][jointname]["max"] = max_sensors(legid, jointid);
      potvals[legname][jointname]["reversed"] = false;
    }
  }
  FILE *fp = fopen("calib_params.json", "w");
  fprintf(fp, "%s", potvals.dump().c_str());
  fclose(fp);
  cout << "[TACHI CALIBRATE] saved information to: calib_params.json\n";
  return 0;
}


#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/Temperature.h>
#include <sensor_msgs/TimeReference.h>
#include <string>
#include <iostream>
#include <fstream>
#include <serial/serial.h>
#include <vector>
#include <sstream>
#include <exception>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/math/constants/constants.hpp>
//#include <chiki-briki_i_v_damki.h>

#include <dynamic_reconfigure/server.h>
#include <mcu_interface/parametersConfig.h>
#include "mcu_interface/AlignMcuCamPhase.h"
#include "mcu_interface/StartMcuCamTriggering.h"
#include "mcu_interface/PublishS10ToMcuOffset.h"

std::string IMU_TOPIC = "mcu_imu";
std::string IMU_TEMP_TOPIC_POSTFIX = "_temp";
std::string CAMERAS_TS_TOPIC = "mcu_cameras_ts";
std::string LIDAR_TS_TOPIC = "mcu_lidar_ts";
std::string S10_TS_TOPIC = "mcu_s10_ts";

std::string IMU_TEMP_TOPIC = IMU_TOPIC + IMU_TEMP_TOPIC_POSTFIX;
std::string PORT;// = "/dev/ttyUSB200";  // port name
const int BAUD = 500000;
int RATE = 10000;
int TIMOUT = 10;
uint32_t FRACT_NUMBER = 25600000;
double G = 9.81;
const double PI = boost::math::constants::pi<double>();
int TEMP_BUF_SIZE = 200;
int NUM_OF_IMUS = 1;
int NUM_OF_TS_FIELDS = 4;
int NUM_OF_IMU_FIELDS = 7;
int PLD_STRT_INDX = 2; // payload starting index in received string line

const uint8_t OUTPUT_DATA_LENGTH_BYTES = 5;
uint8_t ALIGN_FRAMES_CMD = 34;
uint8_t START_TRIGGER_CMD = 56;
uint8_t STOP_TRIGGER_CMD = 57;

uint8_t output_buffer[OUTPUT_DATA_LENGTH_BYTES];
uint32_t CAMERAS_TIM_FRACT_NUMBER = 2560000;
uint32_t alignment_subs = 0;
uint32_t alignment_subs_old = 0;
ros::Time last_cameras_ts = ros::Time(0);
float CAMERAS_FRAME_RATE = 30.0;
float SAMSUNG_CAMERA_FRAME_RATE = 30.0;
float SAMSUNG_CAMERA_FRAMING_PERIOD = 1.0 / SAMSUNG_CAMERA_FRAME_RATE;
double MANUAL_ADDITION = 0.005493;//-0.01243;//0.02064;//+0.0123; //sec
double phase_old = 0;

class FieldsCount{
  public:
    int count;
    FieldsCount (int start = 0) {
        count = start;
    }
    void add (int additive) {
        count += additive;
    }
    int current (void) {
        return count;
    }
};

std::vector<int16_t> string_to_ints(std::string str, int start_from = 0) {
    std::stringstream ss;
    /* Storing the whole string into string stream */
    ss << str.substr(start_from);
    /* Running loop till the end of the stream */
    std::string temp;
    int found;

    std::vector<int16_t> ints;

    while (!ss.eof()) {
        /* extracting word by word from stream */
        ss >> temp;
        /* Checking the given word is integer or not */
        if (std::stringstream(temp) >> std::hex >> found) {
            ints.push_back(static_cast<int16_t>(found));
        }
        temp = "";
    }
    return ints;
}

std::vector<int16_t> subvector(std::vector<int16_t> const &initial_v, int starting_index) {
   std::vector<int16_t> sub_v(initial_v.begin() + starting_index, initial_v.end());
   return sub_v;
}

ros::Time ints_to_board_ts(std::vector<int16_t> input_ints, FieldsCount * fc_pointer, int first_element=0) {
	std::vector<int16_t> ints = subvector(input_ints, fc_pointer->current());

	double secs = static_cast<uint16_t>(ints[0]) * 60.0 + static_cast<uint16_t>(ints[1]) * 1.0 + static_cast<uint32_t>(ints[2]<<16 | static_cast<uint16_t>(ints[3]))*1.0/FRACT_NUMBER;
	ros::Time board_ts = ros::Time(secs);

    fc_pointer->add(NUM_OF_TS_FIELDS);
    return board_ts;
}

boost::numeric::ublas::vector<double> ints_to_imu_meas(std::vector<int16_t> input_ints, FieldsCount * fc_pointer, int first_element=0) {
    std::vector<int16_t> ints = subvector(input_ints, fc_pointer->current());
    boost::numeric::ublas::vector<double> imu_meas(NUM_OF_IMU_FIELDS);
    for (int i = 0; i < imu_meas.size(); i++) {
        // acc
        if (i < 3) {
            imu_meas(i) = ints[i] / 16384.0 * G;
        }
        // temperature
        else if (i == 3) {
            imu_meas(i) = ints[3] / 340.0 + 35.0;
        }
        // gyro
        else if (i >= 3) {
            imu_meas(i) =  ints[i] / 131.0 / 180.0 * PI;
        }
    }

    fc_pointer->add(NUM_OF_IMU_FIELDS);
    return imu_meas;
}

void publish_imu(ros::Publisher pub, uint8_t imu_n, ros::Time ts, boost::numeric::ublas::vector<double> imu_meas) {
    // publish_imu data [a in m/s^2] and [w in rad/s]
    std::string frame_id = IMU_TOPIC + std::to_string(imu_n);
    sensor_msgs::Imu msg;

    msg.header.frame_id = frame_id;
    msg.header.stamp = ts;
    // linear_acceleration
    msg.linear_acceleration.x = imu_meas[0];
    msg.linear_acceleration.y = imu_meas[1];
    msg.linear_acceleration.z = imu_meas[2];
    // angular_velocity
    msg.angular_velocity.x = imu_meas[4];
    msg.angular_velocity.y = imu_meas[5];
    msg.angular_velocity.z = imu_meas[6];
    // Publish the message.
    pub.publish(msg);
}

void publish_imu_temperature(ros::Publisher pub, uint8_t imu_n, ros::Time ts, boost::numeric::ublas::vector<double> imu_meas) {
    std::string frame_id = IMU_TOPIC + std::to_string(imu_n) + IMU_TEMP_TOPIC_POSTFIX;
    sensor_msgs::Temperature msg;

    msg.header.frame_id = frame_id;
    msg.header.stamp = ts;
    msg.temperature = imu_meas[3];

    pub.publish(msg);
}

void publish_cameras_ts(ros::Publisher pub, ros::Time ts) {
    std::string frame_id = CAMERAS_TS_TOPIC;
    sensor_msgs::TimeReference msg;

    msg.header.frame_id = frame_id;
    msg.header.stamp = ts;
    msg.time_ref = ros::Time::now();
    pub.publish(msg);
}

void publish_lidar_ts(ros::Publisher pub, ros::Time ts) {
    std::string frame_id = LIDAR_TS_TOPIC;
    sensor_msgs::TimeReference msg;

    msg.header.frame_id = frame_id;
    msg.header.stamp = ts;
    msg.time_ref = ros::Time::now();
    pub.publish(msg);
}

void publish_s10_ts(ros::Publisher pub, ros::Time mcu_ts, ros::Time s10_ts) {
    std::string frame_id = S10_TS_TOPIC;
    sensor_msgs::TimeReference msg;

    msg.header.frame_id = frame_id;
    msg.header.stamp = mcu_ts;
    msg.time_ref = s10_ts;
    pub.publish(msg);
}

void pub_distributer(std::string str) {
}

// dynamic reconfigure callback
void dynamic_reconfigure_callback(mcu_interface::parametersConfig &config, uint32_t level)
{
    if (config.start_sync)
    {
        // Marsel, here goes the sync function/code only when set to True
        ROS_WARN("dynamic_reconfigure_callback: Flag set to True");//TODO change/remove this
    }
}

void send_to_mcu(serial::Serial *serial, uint8_t cmd, uint32_t data=0) {
    uint8_t buf[OUTPUT_DATA_LENGTH_BYTES];
    buf[0] = cmd;
    for(uint8_t i=1; i<OUTPUT_DATA_LENGTH_BYTES; i++) {
            buf[i] = (uint8_t)(data >> ((i-1) * 8));
    }
    serial->write(buf, OUTPUT_DATA_LENGTH_BYTES);
}
// Service server
bool align_phase(mcu_interface::AlignMcuCamPhase::Request  &req,
         mcu_interface::AlignMcuCamPhase::Response &res, serial::Serial *serial)
{
    double phase = req.a + MANUAL_ADDITION;
    ROS_WARN("phase %f", phase);

    double delta = phase - last_cameras_ts.toSec();
    ROS_WARN("last_cameras_ts.toSec() %f", last_cameras_ts.toSec());
    ROS_WARN("delta = phase - last_ts %f", delta);

    double delta_mod = std::fmod(delta, SAMSUNG_CAMERA_FRAMING_PERIOD);
    ROS_WARN("delta_mod %f", delta_mod);

    double delta_mod_pos = 0;
    if (delta_mod < 0) {
        delta_mod_pos = delta_mod + SAMSUNG_CAMERA_FRAMING_PERIOD;
    }
    ROS_WARN("delta_mod_pos %f", delta_mod_pos);

    double subs = delta_mod_pos * CAMERAS_TIM_FRACT_NUMBER * CAMERAS_FRAME_RATE; //-1
    ROS_WARN("subs %f", subs);

    alignment_subs = static_cast<uint32_t> (std::round(subs));
    //alignment_subs = 0x1A2B3C4D;//static_cast<uint32_t> (std::round(req.a));
    //ROS_WARN("alignment_subs %x\n", alignment_subs);
    ROS_WARN("alignment_subs %d\n", alignment_subs);
    send_to_mcu(serial, ALIGN_FRAMES_CMD, alignment_subs);
    //serial->write((uint8_t *)&alignment_subs, 4);

    // Stub
    res.response = "done";
    //ROS_WARN("sending back response: [%s]", res.response.c_str());

    // Writing debug info into file
    time_t t = time(0);   // get time now
    struct tm * now = localtime( & t );

    char buffer [90];
    strftime(buffer, 90, "/home/mrob/samsung-avatar-dataset-sync/out/mcu_interface/%m(%b)%d_%Y_%H%M%S.csv", now);
    std::ofstream my_file(buffer);
    //my_file.open (buffer);
    //std::string temp(buffer)
    //ROS_WARN("%s", temp.c_str());
    ROS_WARN(buffer);
    if(my_file.is_open()) {
        my_file << "phase," << phase << std::endl;
        my_file << "last_cameras_ts.toSec()," << last_cameras_ts.toSec() << std::endl;
        my_file << "delta = phase - last_ts," << delta << std::endl;
        my_file << "delta_mod," << delta_mod << std::endl;
        my_file << "delta_mod_pos," << delta_mod_pos << std::endl;
        my_file << "subs," << subs << std::endl;
        my_file << "alignment_subs," << alignment_subs << std::endl;
    }
    else {
        ROS_WARN("error");
    }
    my_file.close();

    return true;
}

bool start_triggering(mcu_interface::StartMcuCamTriggering::Request  &req,
         mcu_interface::StartMcuCamTriggering::Response &res, serial::Serial *serial)
{
    send_to_mcu(serial, START_TRIGGER_CMD);

    // Stub
    res.response = "done";
    ROS_WARN("start_triggering OK");
    return true;
}

bool publish_s10_to_mcu_offset(mcu_interface::PublishS10ToMcuOffset::Request  &req,
         mcu_interface::PublishS10ToMcuOffset::Response &res, ros::Publisher *pub)
{
    publish_s10_ts(*pub, ros::Time(last_cameras_ts), ros::Time(last_cameras_ts.toSec() + req.offset));
    // Stub
    res.response = "done";
    //ROS_WARN("sending back response: [%s]", res.response.c_str());
    return true;
}

int main(int argc, char **argv) {
    // Register signal and signal handler
    if (argc < 2) {
        std::cout << "Please, specify serial device. For example, \"/dev/ttyUSB0\"" << std::endl;
        return 0;
    }

    PORT = argv[1];
    bool board_starting_ts_is_read = false;
    std::string str;

    ros::Time sys_starting_ts, board_starting_ts, ts, ts_old;
    ros::Duration delta_ts;
    // Initialize the ROS system and become a node.
    ros::init(argc, argv, "mcu_interface");

    // Create a publisher object.
    ros::NodeHandle nh;
    ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>(IMU_TOPIC, RATE);
	ros::Publisher imu_temp_pub = nh.advertise<sensor_msgs::Temperature>(IMU_TEMP_TOPIC, RATE);
	ros::Publisher cameras_ts_pub = nh.advertise<sensor_msgs::TimeReference>(CAMERAS_TS_TOPIC, RATE);
	ros::Publisher lidar_ts_pub = nh.advertise<sensor_msgs::TimeReference>(LIDAR_TS_TOPIC, RATE);
    ros::Publisher s10_ts_pub = nh.advertise<sensor_msgs::TimeReference>(S10_TS_TOPIC, 2, true);

	// Configure dynamic reconfigure
	dynamic_reconfigure::Server<mcu_interface::parametersConfig> server;
    dynamic_reconfigure::Server<mcu_interface::parametersConfig>::CallbackType f;
    f = boost::bind(&dynamic_reconfigure_callback, _1, _2);
    server.setCallback(f);

	// open port, baudrate, timeout in milliseconds
    serial::Serial serial(PORT, BAUD, serial::Timeout::simpleTimeout(TIMOUT));

    // Service configure
    ros::ServiceServer phase_align_service = nh.advertiseService<mcu_interface::AlignMcuCamPhase::Request, mcu_interface::AlignMcuCamPhase::Response>(
        "align_mcu_cam_phase", boost::bind(align_phase, _1, _2, &serial));

    ros::ServiceServer camera_start_service = nh.advertiseService<mcu_interface::StartMcuCamTriggering::Request, mcu_interface::StartMcuCamTriggering::Response>(
        "start_mcu_cam_triggering", boost::bind(start_triggering, _1, _2, &serial));

    ros::ServiceServer publish_s10_to_mcu_offset_service = nh.advertiseService<mcu_interface::PublishS10ToMcuOffset::Request, mcu_interface::PublishS10ToMcuOffset::Response>(
        "publish_s10_to_mcu_offset", boost::bind(publish_s10_to_mcu_offset, _1, _2, &s10_ts_pub));

    // check if serial port open
    std::cout << "Serial port is...";
    if(serial.isOpen())
        std::cout << " open." << std::endl;
    else
        std::cout << " not open!" << std::endl;

    // Clean from possibly broken string
    while(serial.available() < TEMP_BUF_SIZE) {
        str = serial.readline();
        if(str.at(str.size()-1)=='\n') {
            break;
        }
    }

    //publish_s10_ts(s10_ts_pub, ros::Time(0.5), ros::Time(50));
    //ros::Time some1 = ros::Time::now();
    //uint8_t flag_some = 1;
    //serial.write((uint8_t *)&alignment_subs, 4);
    //send_to_mcu(&serial, START_TRIGGER_CMD);

    // Main loop
    while(ros::ok()) {
        if(serial.available() > TEMP_BUF_SIZE) {
            str = serial.readline();
            //std::cout << str << std::endl;
            std::vector<int16_t> ints = string_to_ints(str, PLD_STRT_INDX);
            FieldsCount fields_count;
            ros::Time ts = ints_to_board_ts(ints, &fields_count);
		   	switch (str.at(0)) {
				case 'i': {
					boost::numeric::ublas::vector<double> imu_meas;
					imu_meas = ints_to_imu_meas(ints, &fields_count);
					publish_imu(imu_pub, 0, ts, imu_meas);
					publish_imu_temperature(imu_temp_pub, 0, ts, imu_meas);
					break;
				}
				case 'c': {
					publish_cameras_ts(cameras_ts_pub, ts);
                    last_cameras_ts = ts;
                    //ROS_WARN("phase=phase*CAMERAS_TIM_FRACT_NUMBER %f", last_cameras_ts.toSec());
					break;
				}
				case 'l': {
                    publish_lidar_ts(lidar_ts_pub, ts);
                    //ROS_WARN("%s", str.c_str());
                    //std::cout << ts << std::endl;
                    //publish_s10_ts(s10_ts_pub, ros::Time(0.5), ros::Time(50));
                    break;
                }
                case 't': {
                    ROS_WARN("%s", str.c_str());
                    break;
                }
			}
			ros::spinOnce();// this checks for callbacks (dynamic reconfigure)
	    }
        //if (flag_some == 1 && (ros::Time::now() - some1).toSec() > 5) {
            //send_to_mcu(&serial, START_TRIGGER_CMD);
            //serial.write((uint8_t *)&alignment_subs, 5);
            //flag_some = 0;
            //ROS_WARN("AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA");
        //}
	    usleep(100);
	}
    send_to_mcu(&serial, STOP_TRIGGER_CMD);
}