#include <Eigen/Dense>
#include <Eigen/Eigen>
#include <Eigen/Geometry>
#include <Eigen/SVD>
#include <fstream>
#include <iostream>
#include <linux/input-event-codes.h>

#include "ros/package.h"
#include "math.h"
#include "myGlobalData.h"
#include "myFunction.h"
#include "origarm_ros/Sensor.h"
#include "origarm_ros/States.h"
#include "origarm_ros/keynumber.h"
#include "origarm_ros/Command_ABL.h"
#include "origarm_ros/Command_Pre_Open.h"
#include "origarm_ros/transdiffConfigSeg.h"
#include "origarm_ros/transdiffConfig.h"
#include "origarm_ros/transdiffDataSeg.h"
#include "origarm_ros/transFFCompensateDataSeg.h"
#include "origarm_ros/transFFCompensateData.h"
#include "origarm_ros/PIDdata.h"

#include "ros/ros.h"
#include <ros/time.h>
#include <time.h>
#include "assert.h"
#include "myPID.h"

#define DEBUG_DISPLAY 0
#define DUMMY_QUAT_USED 0

using namespace Eigen;
using Eigen::Matrix3f;
using Eigen::Matrix4f;
using Eigen::MatrixXf;
using Eigen::Quaternionf;
using Eigen::Vector3f;
using Eigen::VectorXf;

using namespace std;
static float rad2deg = 180.0f / M_PI;

static float alpha[SEGNUM];
static float beta[SEGNUM];
static float lengthAbsM[SEGNUM];

static float alphaDeg[SEGNUM];
static float betaDeg[SEGNUM];
static float lengthAbsMM[SEGNUM];

// local definition of pressure, distance, quaternion
static int16_t pressureActuator[SEGNUM][ACTNUM];
static int16_t lengthActuator[SEGNUM][ACTNUM];

static Quaternionf QuattInIMU[SEGNUM][ACTNUM];
static Quaternionf Quat0InIMU[SEGNUM][ACTNUM];

static Matrix3f Rtrans[SEGNUM][ACTNUM];
static Matrix3f R0InIMU[SEGNUM][ACTNUM], RtInIMU[SEGNUM][ACTNUM];
static Matrix3f R0InArm[SEGNUM][ACTNUM], RtInArm[SEGNUM][ACTNUM];
static Matrix3f dRInArm[SEGNUM][ACTNUM];
static Matrix3f ddRInArm[9][3][SEGNUM];

static float alphaCandidates[9][3][SEGNUM];
static float betaCandidates[9][3][SEGNUM];
static float alphaFromActuators[SEGNUM];
static float betaFromActuators[SEGNUM];

LowPassFilter ablStateFilter[SEGNUM][3];
LowPassFilter ablPassiveFilter[SEGNUM][3];

static Quaternionf quaternionPlates[SEGMENTNUM];
static Matrix3f R0PlateInArm[SEGNUM], RtPlateInArm[SEGNUM], dRPlateInArm[SEGNUM];
static float alphaFromPlates[SEGNUM];
static float betaFromPlates[SEGNUM];

/*Flag to control how the alpha is calculated*/
static const int calculate_alpha_using_pre_cur_flag = 1;
static const int calculate_alpha_using_cur_cur_flag = 0;
static const int calculate_alpha_using_cur_nex_flag = 1;

/*Path to save the IMU data at zero position*/

static string quat0DefaultFileName = "default_data_imu0.txt";
static string quattDummyFileName = "imu_move_segment0.txt";
static string quatSaveFileName = "data_imu0.txt";
static string quatReadFileName = "data_imu0.txt";
static string IMUDataPath = "";
static void InitFrames();

// abl from passive sensing
static float alpha_passive[SEGNUM];
static float beta_passive[SEGNUM];
static float length_passive[SEGNUM];
static float lengthfrompressure[SEGNUM][ACTNUM];
// static float initTransLength[SEGNUM][ACTNUM];   //directly from sensor, relative mm
// static float initTransPressure[SEGNUM][ACTNUM]; //directly from sensor, gauge HPa
static float initTransPressureComp[SEGNUM][ACTNUM];

float measureDeadzoneAlpha = 0.035; // when alphar < measureDeadzoneAlpha, betar = betaD

// pressure compensation for passive sensing actuators

static ros::Time initTransTime0[SEGNUM];
static ros::Duration initTransDuration[SEGNUM];
static float leakageRate[SEGNUM][ACTNUM] = {
    {1, 1, 1, 1, 1, 1},
    {1, 1, 1, 1, 1, 1},
    {0.0021, 1, 0.1, 1, 0.1, 1}, //interested segment
    {1, 1, 1, 1, 1, 1},
    {1, 1, 1, 1, 1, 1},
    {1, 1, 1, 1, 1, 1}};

origarm_ros::transdiffConfig transdiffConfigs;

/*Indication of whether the IMU is working well*/
static int goodIMU[SEGNUM][ACTNUM] = {
    {0, 1, 0, 0, 1, 1},
    {0, 0, 1, 0, 1, 0},
    {1, 1, 1, 0, 1, 1},
    {1, 0, 0, 1, 0, 0},
    {1, 1, 0, 1, 0, 1},
    {0, 1, 0, 0, 1, 0}};

float pressureDFeed[SEGNUM][ACTNUM];
float pressureDBack[SEGNUM][ACTNUM];

static void InitFrames();

std::string getTimeString()
{
    time_t rawtime;
    struct tm *timeinfo;
    char buffer[100];

    time(&rawtime);
    timeinfo = localtime(&rawtime);

    strftime(buffer, 100, "%G_%h_%d_%H_%M_%S", timeinfo);
    std::string ret = buffer;
    return ret;
}

float getPastTime(ros::Time starttime)
{
    ros::Time curtime = ros::Time::now();
    ros::Duration pasttime = curtime - starttime;
    float pastSec = pasttime.toSec();
    return pastSec;
}

/**
 * @brief Save IMU data to file
 * 
 * @param filePath The path where the data is to be stored
 */
static void saveQuatToFile(Quaternionf (&qua)[SEGNUM][ACTNUM], string filePath)
{
    ofstream data;
    data.open(filePath, ios::trunc); // ios::app
    // write imu data into yaml file/imu_data.txt
    cout << "Saving current IMU data to" + filePath << endl;
    for (int i = 0; i < SEGNUM; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            data << qua[i][j].w() << " " << qua[i][j].x() << " " << qua[i][j].y()
                 << " " << qua[i][j].z() << endl;
            cout << qua[i][j].w() << " " << qua[i][j].x() << " " << qua[i][j].y()
                 << " " << qua[i][j].z() << endl;
        }
    }
    data.close();
    cout << "IMU data Saved" << endl;
}

/**
 * @brief read IMU data from file
 * 
 * @param filePath The filepath where the original data is stored
 */
static void readQuatFromFile(string filePath, Quaternionf (&qua)[SEGNUM][ACTNUM])
{
    ifstream inFile;
    inFile.open(filePath, ios::in);
    if (inFile.fail())
    {
        inFile.close();
        cout << "unable to open the IMU file in " << filePath;
        filePath = IMUDataPath + quat0DefaultFileName;
        cout << ". The IMU default values will be used in " << filePath << endl;
        inFile.open(filePath, ios::in);
    }

    cout << "Reading IMU data from" + filePath << endl;
    if (!inFile.eof())
    {
        for (int p = 0; p < SEGNUM; p++)
        {
            for (int q = 0; q < ACTNUM; q++)
            {
                inFile >> qua[p][q].w() >> qua[p][q].x() >> qua[p][q].y() >> qua[p][q].z();
                cout << qua[p][q].w() << " " << qua[p][q].x() << " " << qua[p][q].y()
                     << " " << qua[p][q].z() << endl;
            }
        }
    }
    cout << "IMU data Read completed" << endl;
    inFile.close();
}

/*set DEBUG_DISPLAY=1 to print all the matrices*/
static void displayAllMatrix()
{

    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            cout << "Rtrans Seg " << i << " Act " << j << endl;
            dispMatrix(Rtrans[i][j]);
        }
    }

    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {

            cout << "R0InIMU Seg " << i << " Act " << j << endl;
            cout << Quat0InIMU[i][j].w() << " " << Quat0InIMU[i][j].x() << " " << Quat0InIMU[i][j].y() << " " << Quat0InIMU[i][j].z() << endl;
            dispMatrix(R0InIMU[i][j]);
        }
    }

    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            cout << "RtInIMU Seg " << i << " Act " << j << endl;
            dispMatrix(RtInIMU[i][j]);
        }
    }

    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            cout << "R0InArm Seg " << i << " Act " << j << endl;
            dispMatrix(R0InArm[i][j]);
        }
    }

    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            cout << "RtInArm Seg " << i << " Act " << j << endl;
            dispMatrix(RtInArm[i][j]);
        }
    }

    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            cout << "dRInArm Seg " << i << " Act " << j << endl;
            dispMatrix(dRInArm[i][j]);
        }
    }

    for (int i = 0; i < 6; i++)
    {
        cout << "alphaFromActuator Seg " << i << endl;
        for (int j = 0; j < 3; j++)
        {
            for (int k = 0; k < 9; k++)
            {

                cout << alphaCandidates[k][j][i] << " ";
            }
            cout << endl;
        }
    }
}

void initFilters()
{
    for (int i = 0; i < SEGNUM; i++)
    {
        ablStateFilter[i][0].setCoeff(0.7);
        ablStateFilter[i][1].setCoeff(0.7);
        ablStateFilter[i][2].setCoeff(0.7);
        ablStateFilter[i][0].setValue(0);
        ablStateFilter[i][1].setValue(0);
        ablStateFilter[i][2].setValue(g_length0);

        ablPassiveFilter[i][0].setCoeff(0.7);
        ablPassiveFilter[i][1].setCoeff(0.7);
        ablPassiveFilter[i][2].setCoeff(0.7);
        ablPassiveFilter[i][0].setValue(0);
        ablPassiveFilter[i][1].setValue(0);
        ablPassiveFilter[i][2].setValue(g_length0);
    }
}

class State_Estimator
{
public:
    State_Estimator()
    {
        sub_ = n_.subscribe("Sensor", 300, &State_Estimator::sensorCallback, this);
        key_sub_ = n_.subscribe("key_number", 1, &State_Estimator::keyCallback, this);
        trans_sub_ = n_.subscribe("transdiffConfigSeg", 1, &State_Estimator::transdiffConfigSegCallback, this);
        cmd_sub_ = n_.subscribe("Cmd_ABL_joy", 1, &State_Estimator::ablCmdCallback, this);
        pcmd_sub_ = n_.subscribe("Command_Pre_Open", 1, &State_Estimator::pCmdCallback, this);
        pfeedforward_sub_ = n_.subscribe("Command_Pre_feedforward", 1, &State_Estimator::pfeedforwardCallback, this);
        pfeedback_sub_ = n_.subscribe("Command_Pre_feedback", 1, &State_Estimator::pfeedbackCallback, this);
        transffcompcmd_sub = n_.subscribe("transFFCompData", 1, &State_Estimator::transFFCallback, this);
        piddata_sub_ = n_.subscribe("PID_data", 1, &State_Estimator::PIDdataCallback, this);

        pub_ = n_.advertise<origarm_ros::States>("States", 300);
        pub2_ = n_.advertise<origarm_ros::transdiffConfig>("transdiffConfigurations", 100);
        pub3_ = n_.advertise<origarm_ros::transdiffDataSeg>("transdiffSegmentData", 300);
    }

    void keyCallback(const origarm_ros::keynumber &key)
    {
        if (key.keycodePressed == KEY_C) // 'C' pressed
        {
            printf(" KEY_C pressed!\r\n");
            string path = IMUDataPath + quatSaveFileName;
            saveQuatToFile(QuattInIMU, path);
            InitFrames();
        }
        else if (key.keycodePressed == KEY_R) // 'r' pressed
        {
            printf(" KEY_R pressed! Saving IMU data...\r\n");
            std::string path = IMUDataPath + "Rec_IMU_" + getTimeString() + ".txt";
            saveQuatToFile(QuattInIMU, path);
        }
    }
    void pCmdCallback(const origarm_ros::Command_Pre_Open &msg)
    {
        for (int i = 0; i < SEGNUM; i++)
        {
            for (int j = 0; j < ACTNUM; j++)
            {
                Cmd_pressure.segment[i].command[j].pressure = msg.segment[i].command[j].pressure;
                Cmd_pressure.segment[i].command[j].valve = msg.segment[i].command[j].valve;
            }
        }
    }
    void ablCmdCallback(const origarm_ros::Command_ABL &ablcmd)
    {
        for (int i = 0; i < SEGNUM; i++)
        {
            ablCommand.segment[i].A = ablcmd.segment[i].A;
            ablCommand.segment[i].B = ablcmd.segment[i].B;
            ablCommand.segment[i].L = ablcmd.segment[i].L;
        }
    }
    void sensorCallback(const origarm_ros::Sensor &Sensor_)
    {
        for (int i = 0; i < SEGNUM; i++)
        {
            for (int j = 0; j < ACTNUM; j++)
            {
                pressureActuator[i][j] = Sensor_.sensor_segment[i].sensor_actuator[j].pressure;
                lengthActuator[i][j] = Sensor_.sensor_segment[i].sensor_actuator[j].distance;
                QuattInIMU[i][j].w() = Sensor_.sensor_segment[i].sensor_actuator[j].pose.orientation.w; // double check
                QuattInIMU[i][j].x() = Sensor_.sensor_segment[i].sensor_actuator[j].pose.orientation.x;
                QuattInIMU[i][j].y() = Sensor_.sensor_segment[i].sensor_actuator[j].pose.orientation.y;
                QuattInIMU[i][j].z() = Sensor_.sensor_segment[i].sensor_actuator[j].pose.orientation.z;
                RtInIMU[i][j] = QuattInIMU[i][j];
            }
        }
    }

    void transdiffConfigSegCallback(const origarm_ros::transdiffConfigSeg &msg)
    {
        int segn = msg.transSegment;
        int ret = 0;
        int dataGood = 1;
        int existi[ACTNUM] = {0, 0, 0, 0, 0, 0};
        int existSum = 0;

        //confirm correct and consistent data
        if (!((msg.transSegment >= 0) && (msg.transSegment < SEGNUM) && (msg.transMode >= 0) && (msg.transMode <= 2) && (msg.transActuatorNum >= 0) && (msg.transPassiveNum >= 0) && (msg.transActuatorNum + msg.transPassiveNum <= 6)))
            dataGood = 0;

        if (msg.transMode == 0 && msg.transActuatorNum != ACTNUM)
        {
            dataGood = 0;
        }
        if (msg.transMode == 2 && msg.transActuatorNum == ACTNUM)
        {
            dataGood = 0;
        }
        for (int i = 0; i < ACTNUM; i++)
        {
            for (int j = 0; j < ACTNUM; j++)
            {
                if (i == msg.transActuatorIndex[j])
                {
                    existi[i] = 1;
                }
            }
        }
        // printf("existi= %d %d %d %d %d %d\n", existi[0], existi[1], existi[2], existi[3], existi[4], existi[5]);
        for (int i = 0; i < ACTNUM; i++)
        {
            existSum += existi[i];
        }
        if (existSum != 6)
        {
            dataGood = 0;
        }
        if (dataGood)
        {
            transdiffConfigs.transdiff_Configuration[segn].transMode = msg.transMode;
            transdiffConfigs.transdiff_Configuration[segn].transSegment = msg.transSegment;
            transdiffConfigs.transdiff_Configuration[segn].transActuatorNum = msg.transActuatorNum;
            transdiffConfigs.transdiff_Configuration[segn].transPassiveNum = msg.transPassiveNum;
            for (int j = 0; j < ACTNUM; j++)
            {
                transdiffConfigs.transdiff_Configuration[segn].transActuatorIndex[j] = msg.transActuatorIndex[j];
                if (msg.transMode == 2)
                {
                    transdiffConfigs.transdiff_Configuration[segn].transInitLength[j] = lengthActuator[segn][j];
                    transdiffConfigs.transdiff_Configuration[segn].transInitPressure[j] = pressureActuator[segn][j];
                    initTransTime0[segn] = ros::Time::now(); // for pressure compensation
                }
            }
            pub2_.publish(transdiffConfigs);
            ROS_INFO("Good Transdifferentiation configuration Command");
        }
        else
        {
            ROS_INFO("Incorrect data of Transdifferentiation configuration Command");
        }
    }

    void pfeedforwardCallback(const origarm_ros::Command_Pre_Open &msg)
    {
        for (int i = 0; i < SEGNUM; i++)
        {
            for (int j = 0; j < ACTNUM; j++)
            {
                pressureDFeed[i][j] = msg.segment[i].command[j].pressure;
            }
        }
    }

    void pfeedbackCallback(const origarm_ros::Command_Pre_Open &msg)
    {
        for (int i = 0; i < SEGNUM; i++)
        {
            for (int j = 0; j < ACTNUM; j++)
            {
                pressureDBack[i][j] = msg.segment[i].command[j].pressure;
            }
        }
    }

    void PIDdataCallback(const origarm_ros::PIDdata &msg)
    {
        int segn = 2;
        transdiffSegmentData.dalphaRatio = msg.dalphaRatio[segn];
        transdiffSegmentData.dbeta = msg.dbeta[segn];
        transdiffSegmentData.dlength = msg.dlength[segn];

        transdiffSegmentData.alphaP = msg.alphaP[segn];
        transdiffSegmentData.alphaI = msg.alphaI[segn];
        transdiffSegmentData.alphaD = msg.alphaD[segn];

        transdiffSegmentData.betaP = msg.betaP[segn];
        transdiffSegmentData.betaI = msg.betaI[segn];
        transdiffSegmentData.betaD = msg.betaD[segn];

        transdiffSegmentData.lengthP = msg.lengthP[segn];
        transdiffSegmentData.lengthI = msg.lengthI[segn];
        transdiffSegmentData.lengthD = msg.lengthD[segn];
    }

    void transFFCallback(const origarm_ros::transFFCompensateData &msg)
    {
        int segn = 2;
        transdiffSegmentData.transFFFlag = msg.transFFCData[segn].transFFFlag;
        transdiffSegmentData.pressureCommandFiltered = msg.transFFCData[segn].FFPressure;
        transdiffSegmentData.transFFTalpha = msg.transFFCData[segn].transFFTalpha;
        transdiffSegmentData.transFFTbeta = msg.transFFCData[segn].transFFTbeta;
        for (int j = 0; j < ACTNUM; j++)
        {
            transdiffSegmentData.pressurefeedforwardTrans[j] = msg.transFFCData[segn].transFFPressure[j];
        }
        transdiffSegmentData.vectorAB_Dx = msg.transFFCData[segn].vectorAB_Dx;
        transdiffSegmentData.vectorAB_Dy = msg.transFFCData[segn].vectorAB_Dy;
        transdiffSegmentData.vectorAB_rx = msg.transFFCData[segn].vectorAB_rx;
        transdiffSegmentData.vectorAB_ry = msg.transFFCData[segn].vectorAB_ry;
        transdiffSegmentData.vectorAB_Errx = msg.transFFCData[segn].vectorAB_Errx;
        transdiffSegmentData.vectorAB_Erry = msg.transFFCData[segn].vectorAB_Erry;
        transdiffSegmentData.vectorAB_ErrProjx = msg.transFFCData[segn].vectorAB_ErrProjx;
        transdiffSegmentData.vectorAB_ErrProjy = msg.transFFCData[segn].vectorAB_ErrProjy;
        transdiffSegmentData.vectorAB_FBx = msg.transFFCData[segn].vectorAB_FBx;
        transdiffSegmentData.vectorAB_FBy = msg.transFFCData[segn].vectorAB_FBy;
    }

    void pub()
    {

        for (int i = 0; i < SEGNUM; i++)
        {
            if (transdiffConfigs.transdiff_Configuration[i].transMode == 2)
            {

                states_.ABL.segment[i].A = alpha_passive[i];
                states_.ABL.segment[i].B = beta_passive[i];
                states_.ABL.segment[i].L = length_passive[i];

                if (i == 2)
                {
                    transdiffSegmentData.alphaPassiveFiltered = ablPassiveFilter[i][0].getValue();
                    transdiffSegmentData.betaPassiveFiltered = ablPassiveFilter[i][1].getValue();
                    transdiffSegmentData.lengthPassiveFiltered = ablPassiveFilter[i][2].getValue();
                }
            }
            else
            {
                states_.ABL.segment[i].A = alpha[i];
                states_.ABL.segment[i].B = beta[i];
                states_.ABL.segment[i].L = lengthAbsM[i];

                if (i == 2)
                {
                    transdiffSegmentData.alphaLaserFiltered = ablStateFilter[i][0].getValue();
                    transdiffSegmentData.betaLaserFiltered = ablStateFilter[i][1].getValue();
                    transdiffSegmentData.lengthLaserFiltered = ablStateFilter[i][2].getValue();
                }
            }
        }

        int segn = 2;

        transdiffSegmentData.transMode = transdiffConfigs.transdiff_Configuration[segn].transMode;
        transdiffSegmentData.transSegment = transdiffConfigs.transdiff_Configuration[segn].transSegment;
        transdiffSegmentData.transActuatorNum = transdiffConfigs.transdiff_Configuration[segn].transActuatorNum;
        transdiffSegmentData.transPassiveNum = transdiffConfigs.transdiff_Configuration[segn].transPassiveNum;

        transdiffSegmentData.initTime0 = initTransTime0[segn];
        transdiffSegmentData.passedDuration = initTransDuration[segn];

        transdiffSegmentData.alphaCommand = ablCommand.segment[segn].A;
        transdiffSegmentData.betaCommand = ablCommand.segment[segn].B;
        transdiffSegmentData.lengthCommand = ablCommand.segment[segn].L;
        transdiffSegmentData.alphaPassive = alpha_passive[segn];
        transdiffSegmentData.betaPassive = beta_passive[segn];
        transdiffSegmentData.lengthPassive = length_passive[segn];
        transdiffSegmentData.alphaLaser = alpha[segn];
        transdiffSegmentData.betaLaser = beta[segn];
        transdiffSegmentData.lengthLaser = lengthAbsM[segn];

        for (int j = 0; j < ACTNUM; j++)
        {
            transdiffSegmentData.transActuatorIndex[j] = transdiffConfigs.transdiff_Configuration[segn].transActuatorIndex[j];
            transdiffSegmentData.initPressure[j] = transdiffConfigs.transdiff_Configuration[segn].transInitPressure[j]; //HPa
            transdiffSegmentData.initLength[j] = transdiffConfigs.transdiff_Configuration[segn].transInitLength[j];

            transdiffSegmentData.pressureDecayCoeff[j] = leakageRate[segn][j];
            transdiffSegmentData.initPressureCompensated[j] = initTransPressureComp[segn][j];
            transdiffSegmentData.pressureCommand[j] = Cmd_pressure.segment[segn].command[j].pressure; //HPa
            transdiffSegmentData.pressureCommandType[j] = Cmd_pressure.segment[segn].command[j].valve;
            transdiffSegmentData.pressure[j] = pressureActuator[segn][j]; //HPa
            transdiffSegmentData.lengthFromPressure[j] = (lengthfrompressure[segn][j]) * 1000.0f;
            transdiffSegmentData.lengthFromLaser[j] = lengthActuator[segn][j];

            transdiffSegmentData.pressurefeedforward[j] = pressureDFeed[segn][j]; //HPa
            transdiffSegmentData.pressurefeedback[j] = pressureDBack[segn][j];    //HPa
        }

        // real pose calculated from sensor
        // states_.pose.position.x = ;
        // states_.pose.position.y = ;
        // states_.pose.position.z = ;
        // states_.pose.orientation.w = ;
        // states_.pose.orientation.x = ;
        // states_.pose.orientation.y = ;
        // states_.pose.orientation.z = ;
        pub_.publish(states_);
        pub3_.publish(transdiffSegmentData);
    }

private:
    ros::NodeHandle n_;
    ros::Subscriber sub_;
    ros::Subscriber key_sub_;
    ros::Subscriber trans_sub_;
    ros::Subscriber cmd_sub_;
    ros::Subscriber pcmd_sub_;
    ros::Subscriber pfeedforward_sub_;
    ros::Subscriber pfeedback_sub_;
    ros::Subscriber transffcompcmd_sub;
    ros::Subscriber piddata_sub_;

    ros::Publisher pub_;
    ros::Publisher pub2_;
    ros::Publisher pub3_;
    origarm_ros::States states_;
    origarm_ros::States states_filtered;
    origarm_ros::Command_ABL ablCommand;
    origarm_ros::Command_Pre_Open Cmd_pressure;
    origarm_ros::transdiffDataSeg transdiffSegmentData;
};

/**/
/**
 * @brief Choose good actuators. More complex rules could be added
 * 
 * @param p the segment number
 * @param q the actautor number on the segment
 * @return 0: Bad actuators
 *         1: Good actuators
 */
static int goodActuator(int p, int q)
{
    int ret = 0;
    if (p >= 0 && p < SEGNUM && q >= 0 && q < ACTNUM)
        if (goodIMU[p][q])
            ret = 1;
    return ret;
}

/**
 * @brief Init all frames
 * 1. init the IMU body frame at time 0 in the ARM Base, based on the physical location of the actuators in the arm
 * 2. Read the IMU body frame at time 0 in the IMU Base
 * 3. Calculate the transformation matrix of IMU Base in the ARM Base
 * 4. define the plate pose at time 0 in the Arm base, which are all identities.
 */
void InitFrames()
{
    /*set the IMU body frame at time 0 in the ARM Base*/

    for (int i = 0; i < SEGNUM; i++)
    {
        R0InArm[i][0] = myRotz(-M_PI / 2);
        R0InArm[i][2] = myRotz(-M_PI / 2 + 2 * M_PI / 3);
        R0InArm[i][4] = myRotz(-M_PI / 2 + 4 * M_PI / 3);

        R0InArm[i][1] = myRotz(-M_PI / 6) * myRoty(M_PI);
        R0InArm[i][3] = myRotz(-M_PI / 6 + 2 * M_PI / 3) * myRoty(M_PI);
        R0InArm[i][5] = myRotz(-M_PI / 6 + 4 * M_PI / 3) * myRoty(M_PI);
    }

    /*Read the IMU body frame at time 0 in the IMU Base*/
    string path = IMUDataPath + quatReadFileName;
    readQuatFromFile(path, Quat0InIMU);
    for (int i = 0; i < SEGNUM; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            R0InIMU[i][j] = Quat0InIMU[i][j];
        }
    }

#if DUMMY_QUAT_USED == 1
    /*****************Debug Only.  Uncomment in real application***********************/
    /*Read dummy IMU body frame at time t in the IMU Base*/
    readQuatFromFile(IMUDataPath + quattDummyFileName, QuattInIMU);
    for (int i = 0; i < SEGNUM; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            RtInIMU[i][j] = QuattInIMU[i][j];
        }
    }
#endif

    /*Calculate the transformation matrix of IMU Base in the ARM Base*/
    for (int i = 0; i < SEGNUM; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            Rtrans[i][j] = R0InArm[i][j] * R0InIMU[i][j].transpose();
        }
    }

    /*define the plate pose at time 0 in the Arm base*/
    for (int i = 0; i < SEGNUM; i++)
    {
        R0PlateInArm[i] = Matrix3f::Identity();
    }
}

/*get the IMU body frame at time t in the arm base*/
void getRtInArm()
{
    for (int i = 0; i < SEGNUM; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            RtInArm[i][j] = Rtrans[i][j] * RtInIMU[i][j];
        }
    }
}

/**
 * @brief get the IMU body frame change at time t relative to time 0,  in the Arm Base
 * 
 */
void getdRtInArm()
{
    for (int i = 0; i < SEGNUM; i++)
    {
        for (int j = 0; j < ACTNUM; j++)
        {
            dRInArm[i][j] = RtInArm[i][j] * R0InArm[i][j].transpose();
        }
    }
}

/**
 * @brief get ddRtInArm using different actautor pairs. 
 * There are 3*9 pairs for each segment, that is
 * 1. Using Act(1 3 5) in previous segment and Act(2 4 6) in current segment
 * 2. Using Act(0 2 4) and Act(1 3 5) in current segment
 * 3. Using Act(0 2 4) in current segment and Act(0 2 4) in next segment
 * Only Elegible and good Actuators are considered during the process.
 */
void getddRtInArm()
{
    int p1 = 0; //segment number of the first chosen actuator
    int p2 = 0; //segment number of the second chosen actuator
    int k = 0;  //0~8, representing all the 9 possbile actuator combinations in a given flag

    /*For every segment*/
    for (int i = 0; i < SEGNUM; i++)
    {

        /*Using Act(1 3 5) in previous segment and Act(2 4 6) in current segment*/
        if (calculate_alpha_using_pre_cur_flag)
        {
            k = 0;
            p1 = i - 1;
            p2 = i;
            for (int q1 = 1; q1 < ACTNUM; q1 += 2)
            {
                for (int q2 = 1; q2 < ACTNUM; q2 += 2)
                {
                    /*only Elegible and good Actuator*/
                    if (goodActuator(p1, q1) && goodActuator(p2, q2))
                        ddRInArm[k++][0][i] = dRInArm[p1][q1].transpose() * dRInArm[p2][q2];
                }
            }
        }

        /*Using Act(0 2 4) and Act(1 3 5) in current segment*/
        if (calculate_alpha_using_cur_cur_flag)
        {
            k = 0;
            p1 = i;
            p2 = i;
            for (int q1 = 0; q1 < ACTNUM; q1 += 2)
            {
                for (int q2 = 1; q2 < ACTNUM; q2 += 2)
                {
                    /*only Elegible and good Actuator*/
                    if (goodActuator(p1, q1) && goodActuator(p2, q2))
                        ddRInArm[k++][1][i] = dRInArm[p1][q1].transpose() * dRInArm[p2][q2];
                }
            }
        }
        /*Using Act(0 2 4) in current segment and Act(0 2 4) in next segment*/
        if (calculate_alpha_using_cur_nex_flag)
        {
            k = 0;
            p1 = i;
            p2 = i + 1;
            for (int q1 = 0; q1 < ACTNUM; q1 += 2)
            {
                for (int q2 = 0; q2 < ACTNUM; q2 += 2)
                {
                    /*only Elegible and good Actuator*/
                    if (goodActuator(p1, q1) && goodActuator(p2, q2))
                        ddRInArm[k++][2][i] = dRInArm[p1][q1].transpose() * dRInArm[p2][q2];
                }
            }
        }
    }
}

/**
 * @brief Get the Alpha From R object
 * 
 * @param R 
 * @return float 
 */
float getAlphaFromR(Matrix3f &R)
{
    float val = -1000;
    float alphaCandi = 0;
    if (!R.isZero())
    {
        val = CONSTRAIN(R(2, 2), -1, 1);
        alphaCandi = acos(val);
    }
    else
    {
        alphaCandi = -1000;
    }
    return alphaCandi;
}
/**
 * @brief Get the Beta From a rotation matrix R
 * 
 * @param R The rotation matrix
 * @return float beta
 */
float getBetaFromR(Matrix3f &R)
{
    float alphaCandi = 0;
    float betaCandi = 0;
    alphaCandi = getAlphaFromR(R);
    if (alphaCandi > measureDeadzoneAlpha || alphaCandi < -measureDeadzoneAlpha)
    {
        betaCandi = atan2(R(1, 2) / sin(alphaCandi), R(0, 2) / sin(alphaCandi));
    }

    else
    {
        betaCandi = 0;
    }
    return betaCandi;
}
/**
 * @brief Get the Alpha Beta From Actuators. 
 * For every segment, there are 3*9 = 27 candidate transformation matrix ddR for calculating alpha and beta. 
 * The valid values are chosen and averaged to get the final value
 * 
 */
void getAlphaBetaFromActuators()
{
    float sumAlpha = 0;
    int countsAlpha = 0;
    float sumBeta = 0;
    float countsBeta = 0;
    int average_flag[3] = {calculate_alpha_using_pre_cur_flag, calculate_alpha_using_cur_cur_flag, calculate_alpha_using_cur_nex_flag};

    for (int i = 0; i < SEGNUM; i++)
    {
        sumAlpha = 0;
        countsAlpha = 0;
        sumBeta = 0;
        countsAlpha = 0;
        for (int j = 0; j < 3; j++)
        {
            if (average_flag[j])
            {
                for (int k = 0; k < 9; k++)
                {
                    alphaCandidates[k][j][i] = getAlphaFromR(ddRInArm[k][j][i]);
                    betaCandidates[k][j][i] = getBetaFromR(ddRInArm[k][j][i]);
                    /*unuseful data are set to be -1000*/
                    if (alphaCandidates[k][j][i] > -10)
                    {
                        sumAlpha += alphaCandidates[k][j][i];
                        countsAlpha++;
                        sumBeta += betaCandidates[k][j][i];
                        countsBeta++;
                    }
                }
            }
        }
        alphaFromActuators[i] = sumAlpha / countsAlpha;
        betaFromActuators[i] = sumBeta / countsBeta;
    }
}

/**
 * @brief Average quaternions in an array Quats[], starting from index, with a total number of startnum
 * 
 * @param Quats The quaternion array to be averaged
 * @param startnum The starting index
 * @param num Number of total quaternion to be averaged
 * @return Quaternionf Averaged quaternion
 */
Quaternionf averageQuaternions(Quaternionf *Quats, int startnum, int num)
{
    Quaternionf avQuat;
    Eigen::MatrixXf Qori(4, num);
    Eigen::Matrix4f Qmat;
    Eigen::Vector4f QmatEigenValue;
    Eigen::Matrix4f QmatEigenVector;
    SelfAdjointEigenSolver<Matrix4f> es(Qmat);
    float coeff = 1.0f / num;
    for (int i = 0; i < num; i++)
    {
        Qori(0, i) = Quats[startnum + i].w() * coeff;
        Qori(1, i) = Quats[startnum + i].x() * coeff;
        Qori(2, i) = Quats[startnum + i].y() * coeff;
        Qori(3, i) = Quats[startnum + i].z() * coeff;
    }
    Qmat = Qori * Qori.transpose();
    QmatEigenValue = es.eigenvalues();
    QmatEigenVector = es.eigenvectors();
    avQuat.w() = QmatEigenVector(0, 3);
    avQuat.x() = QmatEigenVector(1, 3);
    avQuat.y() = QmatEigenVector(2, 3);
    avQuat.z() = QmatEigenVector(3, 3);
    return avQuat;
}

/**
 * @brief Get the Plates rotation matrix R in the Arm base
 * The plates quaternions are determined by averaging the quaternions of the actuators on it. Physically these actuator share a common quaternion
 * The choice of Actuators can choose the three actuators on current segment or the next segment.
 */
void getPlatesR()
{
    int use_cur_seg = 1;
    int use_nex_seg = 1;
    Quaternionf quaternionCandi[6];

    int startnum = 0;
    int num = 0;
    for (int i = 0; i < SEGNUM; i++)
    {

        /*The last plate could only use current segment actuators*/
        if (use_cur_seg == 1 || i == SEGNUM - 1)
        {
            quaternionCandi[0] = dRInArm[i][1];
            quaternionCandi[1] = dRInArm[i][3];
            quaternionCandi[2] = dRInArm[i][5];
            startnum = 0;
            num = 3;
        }
        if (use_nex_seg == 1 && (i != SEGNUM - 1))
        {
            quaternionCandi[3] = dRInArm[i + 1][0];
            quaternionCandi[4] = dRInArm[i + 1][2];
            quaternionCandi[5] = dRInArm[i + 1][4];
            startnum += 3;
            num += 3;
        }
        quaternionPlates[i] = averageQuaternions(quaternionCandi, startnum, num);
        RtPlateInArm[i] = quaternionPlates[i];
    }
}

/**
 * @brief Get the Alpha Beta From the relative rotation matrix between plates
 * 
 */
void getAlphaBetaFromPlates()
{
    Matrix3f dRplate;
    for (int i = 0; i < SEGNUM; i++)
    {
        if (i == 0)
            dRplate = RtPlateInArm[i];
        else
            dRplate = RtPlateInArm[i - 1].transpose() * RtPlateInArm[i];
        alphaFromPlates[i] = getAlphaFromR(dRplate);
        betaFromPlates[i] = getBetaFromR(dRplate);
    }
}

/**
 * @brief Get the Alpha of all the segments
 * 
 */
static void getAlpha(float (&alphaCandi)[SEGNUM])
{
    for (int i = 0; i < SEGNUM; i++)
    {
        alpha[i] = alphaCandi[i];
        alphaDeg[i] = alpha[i] * rad2deg;
    }
}

/**
 * @brief Get the Beta af all the segments
 * 
 */
static void getBeta(float (&betaCandi)[SEGNUM])
{
    for (int i = 0; i < SEGNUM; i++)
    {
        beta[i] = betaCandi[i];
        betaDeg[i] = betaCandi[i] * rad2deg;
    }
}

/**
 * @brief Get the Beta af all the segments
 * 
 */
// static void getBetaFromLength(float (&lengthAbsMM)[SEGNUM])
// {
//     for (int i = 0; i < SEGNUM; i++)
//     {
//         beta[i] = betaCandi[i];
//         betaDeg[i] = betaCandi[i] * rad2deg;
//     }
// }

static void getLength()
{
    float lengthSum = 0;
    for (int i = 0; i < SEGNUM; i++)
    {
        if (i > 5)
        {
            lengthAbsMM[i] = g_length0MM;
            lengthAbsM[i] = g_length0;
        }
        else
        {
            lengthSum = 0;
            for (int j = 0; j < ACTNUM; j++)
            {
                lengthSum += lengthActuator[i][j];
            }

            lengthAbsMM[i] = lengthSum / ACTNUM + g_length0MM;
            lengthAbsM[i] = lengthAbsMM[i] / 1000.0f;
        }
    }
}

/**
 * @brief Estmate the alpha, beta, lengthAbsM from IMU data for all the segments
 * 
 */
void estimateABL()
{
    /*Calculation of useful matrices*/
    getRtInArm();
    getdRtInArm();
    getddRtInArm();

    /*Two methods to obtain alpha and beta*/
    getAlphaBetaFromActuators();
    getAlphaBetaFromPlates();

    /*Choose a result for alpha and beta*/
    getAlpha(alphaFromActuators);
    getBeta(betaFromActuators);

    /*estimate length*/
    getLength();
}

static void getArcLength(float (&rotangle)[SEGNUM])
{
    float lengthSum, Length1, Length2, lambda, arcLength;
    for (int i = 0; i < SEGNUM; i++)
    {
        if (i > 5)
        {
            lengthAbsMM[i] = g_length0MM;
            lengthAbsM[i] = g_length0;
        }
        else
        {
            lengthSum = 0;
            for (int j = 0; j < ACTNUM; j++)
            {
                lengthSum += lengthActuator[i][j];
            }

            if (rotangle[i] == 0)
            {
                lengthAbsMM[i] = lengthSum / ACTNUM + g_length0MM;
                lengthAbsM[i] = lengthAbsMM[i] / 1000.0f;
            }
            else
            {
                Length1 = (lengthActuator[i][0] + lengthActuator[i][2] + lengthActuator[i][4]) / 3;
                Length2 = (lengthActuator[i][1] + lengthActuator[i][3] + lengthActuator[i][5]) / 3;
                lambda = (Length1 - Length2 * cos(rotangle[i])) / (1 - cos(rotangle[i]) * cos(rotangle[i]));
                arcLength = (lambda * cos(rotangle[i]) * rotangle[i]) / (2 * sin(rotangle[i] / 2));
                lengthAbsMM[i] = arcLength + g_length0MM;
                lengthAbsM[i] = arcLength / 1000.0f;
            }
        }
    }
}

Eigen::Vector3f getNormalVectfromCovMatrix(Eigen::Matrix3f &CovM)
{
    Matrix3f U, S, V;
    Vector3f normalV;
    JacobiSVD<Eigen::Matrix3f> svd(CovM, ComputeFullU | ComputeFullV);

    static Eigen::IOFormat dispMatFormat(4, 0, ", ", "\n", "[", "]");
    static std::string separator = "\n---------------------------------------\n";

    U = svd.matrixU();
    V = svd.matrixV();

    // normal direction of plate should point towards +z
    if (V(2, 2) < 0)
    {
        normalV(0, 0) = -V(0, 2);
        normalV(1, 0) = -V(1, 2);
        normalV(2, 0) = -V(2, 2);
    }
    else
    {
        normalV(0, 0) = V(0, 2);
        normalV(1, 0) = V(1, 2);
        normalV(2, 0) = V(2, 2);
    }

    return normalV;
}

Eigen::Matrix3f getdRplatefromNormalVect(Eigen::Vector3f &Vn)
{
    Vector3f V0, crossV;
    Matrix3f dRplate;
    float rotangle;

    V0 << 0,
        0,
        1;

    if ((Vn - V0).isMuchSmallerThan(1e-3))
    {
        crossV = V0;
    }
    else
    {
        crossV = V0.cross(Vn);
    }

    rotangle = acos(Vn.dot(V0));
    dRplate = AngleAxisf(rotangle, crossV);

    return dRplate;
}

void getAlphaBetafromLaser()
{
    MatrixXf A(ACTNUM, 3);
    Matrix3f Q, dRplate;
    Vector3f NormalVect;
    float Zc[SEGNUM], z[SEGNUM][ACTNUM];
    float Zsum;

    NormalVect << 0,
        0,
        1;

    // compute middle point
    for (int i = 0; i < SEGNUM; i++)
    {
        if (i > 5)
        {
            alphaFromPlates[i] = 0;
            betaFromPlates[i] = 0;
        }
        else
        {
            Zsum = 0;
            for (int j = 0; j < ACTNUM; j++)
            {
                Zsum += lengthActuator[i][j];
                z[i][j] = lengthActuator[i][j];
            }

            Zc[i] = Zsum / ACTNUM;

            // printf("Xc[%d]: %f, Yc[%d]: %f, Zc[%d]: %f\n",i, Xc[i], i, Yc[i], i, Zc[i]);

            A(0, 0) = cos(0 * M_PI / 3) * g_install_radiusMM;
            A(0, 1) = sin(0 * M_PI / 3) * g_install_radiusMM;
            A(0, 2) = z[i][0] - Zc[i];
            A(1, 0) = cos(1 * M_PI / 3) * g_install_radiusMM;
            A(1, 1) = sin(1 * M_PI / 3) * g_install_radiusMM;
            A(1, 2) = z[i][1] - Zc[i];
            A(2, 0) = cos(2 * M_PI / 3) * g_install_radiusMM;
            A(2, 1) = sin(2 * M_PI / 3) * g_install_radiusMM;
            A(2, 2) = z[i][2] - Zc[i];
            A(3, 0) = cos(3 * M_PI / 3) * g_install_radiusMM;
            A(3, 1) = sin(3 * M_PI / 3) * g_install_radiusMM;
            A(3, 2) = z[i][3] - Zc[i];
            A(4, 0) = cos(4 * M_PI / 3) * g_install_radiusMM;
            A(4, 1) = sin(4 * M_PI / 3) * g_install_radiusMM;
            A(4, 2) = z[i][4] - Zc[i];
            A(5, 0) = cos(5 * M_PI / 3) * g_install_radiusMM;
            A(5, 1) = sin(5 * M_PI / 3) * g_install_radiusMM;
            A(5, 2) = z[i][5] - Zc[i];

            // covariance matrix Q, to minimize |QX|, where X is normal vector of plate
            Q = (A.transpose() * A) / ACTNUM;

            NormalVect = getNormalVectfromCovMatrix(Q);
            dRplate = getdRplatefromNormalVect(NormalVect);
            alphaFromPlates[i] = getAlphaFromR(dRplate);
            betaFromPlates[i] = getBetaFromR(dRplate);

            // printf("alphaFromLength[%d]: %f\n", i, alphaFromPlates[i]);
            // printf("betaFromLength[%d]: %f\n", i, betaFromPlates[i]);
        }
    }
}

void getABLfromLaser()
{
    /*estimate alpha & beta*/
    getAlphaBetafromLaser();
    getAlpha(alphaFromPlates);
    getBeta(betaFromPlates);

    /*estimate length*/
    getLength();
    // getArcLength(alphaFromPlates);
}

/**
 * @brief Get the Gauge Pressure With Leakage consideration
 * 
 * @param p0 The initial gauge pressure of the closed actuator
 * @param elapsedT passsing time (seconds)
 * @param lrate leakage rate, either positive or negative value is OK.
 * @return float 
 */
float getGaugePressureWithLeakage(float p0, float elapsedT, float lrate)
{
    elapsedT = abs(elapsedT);
    lrate = -abs(lrate);
    return p0 * exp(lrate * elapsedT);
}

/**
 * @brief Get the Compensated InitTransPressure for certain actuator considering the leakage
 * 
 * @param segn segment number
 * @param actn actuator number
 * @return float leakage compensated pressure
 */
float getCompensatedInitTransPressure(int segn, int actn)
{
    ros::Time curT = ros::Time::now();
    initTransDuration[segn] = curT - initTransTime0[segn];
    float passt = initTransDuration[segn].toSec();
    return getGaugePressureWithLeakage(transdiffConfigs.transdiff_Configuration[segn].transInitPressure[actn], passt, leakageRate[segn][actn]); //gauge hpa
}

/**
 * @brief Get the Length From Pressure object within a closed chamber
 *        Based on PV=nRT, the nominal equation is P0*(A*L0 + deadV) =Pt*(A*Lt + deadV), and the result is  Lt=(P0/Pt)*(L0 + d/A)-(d/A). 
 *        If the leakage of the chamber is considered, then the equation should be modified as a time-decaying P0t*(A*L0 + deadV) =Pt*(A*Lt + deadV).
 *        P0t could be calculated from the The leakage rate k which is measured by experiments.
 *        Both P and L shoule be changed to absolute value
 * @param segn 
 * @param actn 
 * @return float relative lengthRelM in relative m
 */
float getLengthFromPressure(int segn, int actn)
{

    static float deadVolume = M_PI * 0.00125 * 0.00125 * 2 * 1.5; //4mm tube (2.5mm inner diameter, 1.5m length)

    float Aequal = g_actuator_effectiveArea * 1.2;
    float L0equal = g_length0 * 1.1;
    float deadVolumeOverArea = deadVolume / Aequal;

    initTransPressureComp[segn][actn] = getCompensatedInitTransPressure(segn, actn);
    float dLFromZero = ((initTransPressureComp[segn][actn] * 100 + 101325) / (pressureActuator[segn][actn] * 100 + 101325) * ((transdiffConfigs.transdiff_Configuration[segn].transInitLength[actn] / 1000.0f + L0equal) + deadVolumeOverArea) - deadVolumeOverArea) - L0equal;
    float dLFromInit = dLFromZero-transdiffConfigs.transdiff_Configuration[segn].transInitLength[actn] / 1000.0f;
    if (actn == 2 || actn == 4)
    {
        dLFromInit = dLFromInit * 1.5;
    }
    dLFromZero=dLFromInit+transdiffConfigs.transdiff_Configuration[segn].transInitLength[actn] / 1000.0f;
    return dLFromZero;
}

Vector3f getNormalVecFromPoints(const Ref<const MatrixXf> &pointsMat, int pointsNum)
{
    static Matrix3f Q;
    static Vector3f Vn;
    Q = (pointsMat.transpose() * pointsMat) / pointsNum;
    Vn = getNormalVectfromCovMatrix(Q);
    return Vn;
}

/**
 * @brief Get Alpha Beta Length From Points 
 * 
 * @param pointsMat the points matrix with dimension(pointsNum,3), with zero mean in Z axis
 * @param pointsNum total number of points
 * @param optBeta optional supplymentary beta information
 * @param pointsInd index of two points
 * @return Vector3f 
 */
Vector3f getStatesFromPoints(Ref<MatrixXf> pointsMat, int pointsNum, float optBeta, int *pointsInd)
{

    Vector3f Vnorm, Vret;
    static Matrix3f dRplateLocal;
    float lengthMean;

    if (pointsNum == 2)
    {
        float oa1 = abs(cos(g_install_angles[pointsInd[0]] - optBeta) * g_install_radius);
        float ob1 = abs(-cos(g_install_angles[pointsInd[1]] - optBeta) * g_install_radius);
        float length1 = pointsMat(0, 2);
        float length2 = pointsMat(1, 2);
        float d = 0;
        float tanAlpha = 0;
        float al;
        float len;

        if (length1 != length2)
        {
            d = abs((length2 * (oa1 + ob1) / (length1 - length2)));
            tanAlpha = abs(length2 / d);
            al = atan(tanAlpha);
            if (length1 > length2)
            {
                len = tanAlpha * (d + ob1);
            }
            else
            {
                len = tanAlpha * (d - ob1);
            }
        }
        else
        {
            len = length1;
            al = 0;
        }

        printf("length1:%3.3f, length2:%3.3f d=%3.3f tanAlpha=%3.3f alpha=%3.3f len=%3.3f optBeta=%3.3f \n",
               length1,
               length2,
               d,
               tanAlpha,
               al,
               len,
               optBeta);

        Vret(0) = al;
        Vret(1) = optBeta;
        Vret(2) = len;
    }
    else
    {
        //length
        lengthMean = pointsMat.col(2).mean();

        Vret(2) = lengthMean; //absolute M
        // cout << "pointsMat" << pointsMat << endl;
        // cout << "pointsMatCol(2)=" << pointsMat.col(2) << endl;
        // cout << "pointsMatCol(2).mean()=" << lengthMean << endl;

        //zero mean of Z axis for the points matrix
        for (int i = 0; i < pointsNum; i++)
        {
            pointsMat(i, 2) -= lengthMean;
        }

        Vnorm = getNormalVecFromPoints(pointsMat, pointsNum);
        // cout << "Vnorm=" << Vnorm << endl;
        dRplateLocal = getdRplatefromNormalVect(Vnorm);
        // cout << "Rplate=" << dRplateLocal << endl;
        //alpha
        Vret(0) = getAlphaFromR(dRplateLocal);

        //beta
        Vret(1) = getBetaFromR(dRplateLocal);

        // cout << "Vret=" << Vret << endl;
    }

    return Vret;
}

/**
 * @brief Estimate the alpha, beta, length from pressure data for certain segments
 * 
 */
static Vector3f passiveSensingOneSeg(int segn)
{
    static Matrix3f Q, dRplate;

    int actuatorNum = transdiffConfigs.transdiff_Configuration[segn].transActuatorNum;
    int passiveActNum = transdiffConfigs.transdiff_Configuration[segn].transPassiveNum;

    MatrixXf A = MatrixXf::Zero(passiveActNum, 3);
    float lengthsum, lengthc[SEGNUM];

    Vector3f statePassive;

    int passiveActIndex[ACTNUM] = {0, 0, 0, 0, 0, 0};

    //update local PassiveIndexBuffer
    for (int i = 0; i < passiveActNum; i++)
    {
        passiveActIndex[i] = transdiffConfigs.transdiff_Configuration[segn].transActuatorIndex[i + actuatorNum];
    }

    for (int i = 0; i < passiveActNum; i++)
    {
        lengthfrompressure[segn][passiveActIndex[i]] = getLengthFromPressure(segn, passiveActIndex[i]);
    }

    //points coordinates
    for (int i = 0; i < passiveActNum; i++)
    {
        A(i, 0) = cos(g_install_angles[passiveActIndex[i]]) * g_install_radius;
        A(i, 1) = sin(g_install_angles[passiveActIndex[i]]) * g_install_radius;
        A(i, 2) = lengthfrompressure[segn][passiveActIndex[i]] + g_length0;
    }

    if (passiveActNum >= 3)
    {
        // cout << "A=" << A << endl;
        // cout << "passiveActNum" << passiveActNum << endl;
        // printf("passiveActIndex %d %d %d %d %d %d\n", passiveActIndex[0], passiveActIndex[1], passiveActIndex[2], passiveActIndex[3], passiveActIndex[4], passiveActIndex[5]);

        statePassive = getStatesFromPoints(A, passiveActNum, 0, passiveActIndex);
    }
    else
    {
        float deducedBeta = 0;
        float pcos = 0;
        float psin = 0;
        for (int i = 0; i < transdiffConfigs.transdiff_Configuration[segn].transActuatorNum; i++)
        {
            int ind = transdiffConfigs.transdiff_Configuration[segn].transActuatorIndex[i];
            pcos += pressureActuator[segn][ind] * cos(g_install_angles[ind]);
            psin += pressureActuator[segn][ind] * sin(g_install_angles[ind]);
        }
        deducedBeta = atan2(psin, pcos) - M_PI;
        deducedBeta = 0;
        statePassive = getStatesFromPoints(A, passiveActNum, deducedBeta, passiveActIndex);
    }
    return statePassive;
}

void transSegmentsPassiveSensing()
{
    // for (int i = 0; i < SEGNUM; i++)
    // {
    //     printf("transdiffConfigs in StateEstimator, Seg %d, Mode %d, ActNum %d, PassiveNum %d, ActIndex %d %d %d %d %d %d\n",
    //            transdiffConfigs.transdiff_Configuration[i].transSegment,
    //            transdiffConfigs.transdiff_Configuration[i].transMode,
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorNum,
    //            transdiffConfigs.transdiff_Configuration[i].transPassiveNum,
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[0],
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[1],
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[2],
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[3],
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[4],
    //            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[5]);
    // }

    Vector3f statePassive;
    for (int i = 0; i < SEGNUM; i++)
    {
        if (i < 6)
        {
            if (transdiffConfigs.transdiff_Configuration[i].transMode == 2)
            {
                statePassive = passiveSensingOneSeg(i);
                alpha_passive[i] = statePassive(0);
                beta_passive[i] = statePassive(1);
                length_passive[i] = statePassive(2);
            }
        }
    }
}

void initTransParameters()
{
    for (int i = 0; i < SEGNUM; i++)
    {
        transdiffConfigs.transdiff_Configuration[i].transMode = 0;
        transdiffConfigs.transdiff_Configuration[i].transSegment = i;
        transdiffConfigs.transdiff_Configuration[i].transActuatorNum = 6;
        transdiffConfigs.transdiff_Configuration[i].transPassiveNum = 0;
        for (int j = 0; j < ACTNUM; j++)
        {
            transdiffConfigs.transdiff_Configuration[i].transActuatorIndex[j] = j;
        }
    }
}

void filterLaserState()
{
    for (int i = 0; i < SEGNUM; i++)
    {
        ablStateFilter[i][0].update(alpha[i]);
        ablStateFilter[i][1].update(beta[i]);
        ablStateFilter[i][2].update(lengthAbsM[i]);
        ablPassiveFilter[i][0].update(alpha_passive[i]);
        ablPassiveFilter[i][1].update(beta_passive[i]);
        ablPassiveFilter[i][2].update(length_passive[i]);
    }
}

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

    IMUDataPath = ros::package::getPath("origarm_ros") + "/predefined_param/";
    cout << "IMUDataPath:" << IMUDataPath << endl;

    InitFrames();
    initTransParameters();
    initFilters();
    State_Estimator stateEstimator;

    ros::AsyncSpinner s(0);

    s.start();

    ros::Rate loop_rate(100);

    ROS_INFO("Ready for State_Estimator_Node");

    int printFre = 0;
    // BeginTime = ros::Time::now();
    while (ros::ok())
    {
        //estimateABL();
        getABLfromLaser();

        transSegmentsPassiveSensing();

        filterLaserState();

        stateEstimator.pub();

        // if (printFre++ > 2)
        // {
        //     printf("A[0]:%.3f, [1]:%.3f, [2]:%.3f, [3]:%.3f, [4]:%.3f, [5]:%.3f  | B[0]:%.3f, [1]:%.3f, [2]:%.3f, [3]:%.3f, [4]:%.3f, [5]:%.3f  |  L[0]:%d, [1]:%d, [2]:%d, [3]:%d, [4]:%d, [5]:%d\n",
        //            (alpha[0]),
        //            (alpha[1]),
        //            (alpha[2]),
        //            (alpha[3]),
        //            (alpha[4]),
        //            (alpha[5]),
        //            (beta[0]),
        //            (beta[1]),
        //            (beta[2]),
        //            (beta[3]),
        //            (beta[4]),
        //            (beta[5]),
        //            (int)(lengthAbsMM[0]),
        //            (int)(lengthAbsMM[1]),
        //            (int)(lengthAbsMM[2]),
        //            (int)(lengthAbsMM[3]),
        //            (int)(lengthAbsMM[4]),
        //            (int)(lengthAbsMM[5]));
        //     printFre = 0;
        // }

        // if (printFre++ > 2)
        // {
        //     printf("Time:%s\r\n", getTimeString().c_str());
        //     printf("ABLfromLaser        | ABLfromPassiveSensing        | LengthfromPressure\r\n");
        //     for (int i = 0; i < 6; i++)
        //     {
        //         printf("Data[%d]: %.3f %.3f %.3f |%.3f %.3f %.3f |%.3f %.3f %.3f %.3f %.3f %.3f\r\n", i,
        //                alpha[i], beta[i], lengthAbsM[i],
        //                alpha_passive[i], beta_passive[i], length_passive[i],
        //                lengthfrompressure[i][0],
        //                lengthfrompressure[i][1],
        //                lengthfrompressure[i][2],
        //                lengthfrompressure[i][3],
        //                lengthfrompressure[i][4],
        //                lengthfrompressure[i][5]);
        //     }
        //     printFre = 0;
        // }

#if DEBUG_DISPLAY == 1
        displayAllMatrix();
#endif
        ros::spinOnce();
        loop_rate.sleep();
    }

    return 0;
}
