// #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 "myData.h"
// #include "myFunction.h"
// #include "origarm_ros/Sensor.h"
// #include "origarm_ros/States.h"
// #include "origarm_ros/keynumber.h"
// #include "origarm_ros/modenumber.h"
// #include "origarm_ros/segnumber.h"
// #include "origarm_ros/transdiff.h"
// #include "ros/ros.h"
// #include <time.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;

// // abl from sensor
// static float alpha[SEGNUM];
// static float beta[SEGNUM];
// static float length[SEGNUM];

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

// // abl from passive sensing
// static float alpha_passive[SEGNUM];
// static float beta_passive[SEGNUM];
// static float length_passive[SEGNUM];
// static float initLengthTrans[SEGNUM][ACTNUM];
// static float initPressureTrans[SEGNUM][ACTNUM];
// static float deadVolume = M_PI * 0.00125 * 0.00125 * 1.5;
// static float lengthfrompressure[SEGNUM][ACTNUM];
// static int transSegmentFlag[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];

// 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();

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

// //stiffness of each actuator, unit: hPa/m
// static float stiffnessMatrix[SEGNUM][ACTNUM] = {
//     {3000, 3000, 3000, 3000, 3000, 3000},
//     {3000, 3000, 3000, 3000, 3000, 3000},
//     {3000, 3000, 3000, 3000, 3000, 3000},
//     {3000, 3000, 3000, 3000, 3000, 3000},
//     {3000, 3000, 3000, 3000, 3000, 3000},
//     {3000, 3000, 3000, 3000, 3000, 3000}};

// // mode indicating whether its normal or failure or with passive sensing
// static int mode_;
// // segnumber indicating sensors of which segment are shutdown
// static int segn_;
// // actuator transfering from actuation to sensing
// static int trans_actuator;

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

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

// class State_Estimator
// {
// public:
//     State_Estimator()
//     {
//         sub_ = n_.subscribe("Sensor", 300, &State_Estimator::callback, this);
//         key_sub_ = n_.subscribe("key_number", 1, &State_Estimator::keyCallback, this);
//         mode_sub_ = n_.subscribe("modenumber", 1, &State_Estimator::modeCallback, this);
//         segn_sub_ = n_.subscribe("segnumber", 1, &State_Estimator::segnumberCallback, this);
//         trans_sub_ = n_.subscribe("transdiffer", 1, &State_Estimator::transdiffCallback, this);
//         pub_ = n_.advertise<origarm_ros::States>("States", 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 callback(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 modeCallback(const origarm_ros::modenumber &mode_msg)
//     {
//         mode_ = mode_msg.modeNumber;
//     }

//     void segnumberCallback(const origarm_ros::segnumber &segn_msg)
//     {
//         segn_ = segn_msg.segmentNumber;
//     }

//     void transdiffCallback(const origarm_ros::transdiff &msg)
//     {

//     }

//     void pub()
//     {
//         if (mode_ < 2) // real ABL calculated from sensor
//         {
//             for (int i = 0; i < SEGNUM; i++)
//             {
//                 states_.ABL.segment[i].A = alpha[i];
//                 states_.ABL.segment[i].B = beta[i];
//                 states_.ABL.segment[i].L = length[i];
//             }
//         }
//         else if (mode_ == 2)
//         {
//             for (int i = 0; i < SEGNUM; i++)
//             {
//                 if (i == segn_) // real ABL calculated from passive sensing
//                 {
//                     states_.ABL.segment[i].A = alpha_passive[i];
//                     states_.ABL.segment[i].B = beta_passive[i];
//                     states_.ABL.segment[i].L = length_passive[i];

//                     printf("ABL of segment[%d] from passive sensing!\n", i);
//                 }
//                 else
//                 {
//                     states_.ABL.segment[i].A = alpha[i];
//                     states_.ABL.segment[i].B = beta[i];
//                     states_.ABL.segment[i].L = length[i];
//                 }
//             }
//         }

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

// private:
//     ros::NodeHandle n_;
//     ros::Subscriber sub_;
//     ros::Subscriber key_sub_;
//     ros::Subscriber mode_sub_;
//     ros::Subscriber segn_sub_;
//     ros::Subscriber trans_sub_;
//     ros::Publisher pub_;

//     origarm_ros::States states_;
// };

// /**/
// /**
//  * @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 > 0.05f || alphaCandi < -0.05f)
//         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 Length of all the segments
//  *
//  */
// static void getLength()
// {
//     float lengthSum = 0;
//     for (int i = 0; i < SEGNUM; i++)
//     {
//         if (i > 5)
//         {
//             lengthAbsMM[i] = g_length0MM;
//             length[i] = g_length0;
//         }
//         else
//         {
//             lengthSum = 0;
//             for (int j = 0; j < ACTNUM; j++)
//             {
//                 lengthSum += lengthActuator[i][j];
//             }

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

// /**
//  * @brief Estmate the alpha, beta, length 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();
// }

// /**
//  * @brief Estimate the alpha, beta, length from laser sensor
//  *
//  */
// void test()
// {
//     for (int i = 0; i < SEGNUM; i++)
//     {
//         lengthActuator[i][0] = 51 - g_length0MM;
//         lengthActuator[i][1] = 6 - g_length0MM;
//         lengthActuator[i][2] = 10 - g_length0MM;
//         lengthActuator[i][3] = 59 - g_length0MM;
//         lengthActuator[i][4] = 103 - g_length0MM;
//         lengthActuator[i][5] = 99 - g_length0MM;
//     }
// }

// 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;
//             length[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;
//                 length[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;
//                 length[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) * radRmm;
//             A(0, 1) = sin(0 * M_PI / 3) * radRmm;
//             A(0, 2) = z[i][0] - Zc[i];
//             A(1, 0) = cos(1 * M_PI / 3) * radRmm;
//             A(1, 1) = sin(1 * M_PI / 3) * radRmm;
//             A(1, 2) = z[i][1] - Zc[i];
//             A(2, 0) = cos(2 * M_PI / 3) * radRmm;
//             A(2, 1) = sin(2 * M_PI / 3) * radRmm;
//             A(2, 2) = z[i][2] - Zc[i];
//             A(3, 0) = cos(3 * M_PI / 3) * radRmm;
//             A(3, 1) = sin(3 * M_PI / 3) * radRmm;
//             A(3, 2) = z[i][3] - Zc[i];
//             A(4, 0) = cos(4 * M_PI / 3) * radRmm;
//             A(4, 1) = sin(4 * M_PI / 3) * radRmm;
//             A(4, 2) = z[i][4] - Zc[i];
//             A(5, 0) = cos(5 * M_PI / 3) * radRmm;
//             A(5, 1) = sin(5 * M_PI / 3) * radRmm;
//             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);
// }

// void getLengthFromPressure(int segn,int actn){
//     //P1*(A*L1 + deadV) =P2*(A*L2 + deadV)
//     //L2=P1/P2*L1 + (P1/P2-1)*(d/A)
//     float temp1=initPressureTrans[segn][actn]/pressureActuator[segn][actn];
//     float temp2=deadVolume/g_actuator_effectiveArea;
//     lengthfrompressure[segn][actn]=temp1*initLengthTrans[segn][actn]+(temp1-1)*temp2;
// }

// void transCommandCallback(){
//     if(trans_mode==2){
//         transSegmentFlag[trans_segment]=1;
//         for(int i=0;i<ACTNUM;i++){
//             initLengthTrans[trans_segment][i]=lengthActuator[trans_segment][i];
//             initPressureTrans[trans_segment][i]=pressureActuator[trans_segment][i];
//         }

//     }
//     else{
//         transSegmentFlag[trans_segment]=0;
//     }
// }
// /**
//  * @brief Estimate the alpha, beta, length from pressure data for certain segments
//  *
//  */
// static void passiveSensingOneSeg(int transSeg)
// {
//     Matrix3f A, Q, dRplate;
//     Vector3f Vn;
//     float lengthsum, lengthc[SEGNUM]

//     alpha_passive[transSeg] = 0;
//     beta_passive[transSeg] = 0;
//     length_passive[transSeg] = g_length0;

//     // close [0], [2], [4] actuator for passive sensing
//     lengthfrompressure[transSeg][0] = getLengthFromPressure(transSeg,0);
//     lengthfrompressure[transSeg][2] = getLengthFromPressure(transSeg,2);
//     lengthfrompressure[transSeg][4] = getLengthFromPressure(transSeg,4);

//     // for testing
//     // lengthfrompressure[transSeg][0] = lengthActuator[transSeg][0];
//     // lengthfrompressure[transSeg][2] = lengthActuator[transSeg][2];
//     // lengthfrompressure[transSeg][4] = lengthActuator[transSeg][4];
//     lengthsum = lengthfrompressure[transSeg][0] + lengthfrompressure[transSeg][2] + lengthfrompressure[transSeg][4];

//     lengthc[transSeg] = lengthsum / 3;
//     A(0, 0) = cos(0 * M_PI / 3) * radRmm;
//     A(0, 1) = sin(0 * M_PI / 3) * radRmm;
//     A(0, 2) = lengthfrompressure[transSeg][0] - lengthc[transSeg];
//     A(1, 0) = cos(2 * M_PI / 3) * radRmm;
//     A(1, 1) = sin(2 * M_PI / 3) * radRmm;
//     A(1, 2) = lengthfrompressure[transSeg][2] - lengthc[transSeg];
//     A(2, 0) = cos(4 * M_PI / 3) * radRmm;
//     A(2, 1) = sin(4 * M_PI / 3) * radRmm;
//     A(2, 2) = lengthfrompressure[transSeg][4] - lengthc[transSeg];
//     Q = (A.transpose() * A) / 3;
//     Vn = getNormalVectfromCovMatrix(Q);
//     dRplate = getdRplatefromNormalVect(Vn);
//     alpha_passive[transSeg] = getAlphaFromR(dRplate);
//     beta_passive[transSeg] = getBetaFromR(dRplate);
//     length_passive[transSeg] = lengthc[transSeg] + g_length0; //unit: m

//     // printf("alpha_passive[%d]: %f\n", transSeg, alpha_passive[transSeg]);
//     // printf("beta_passive[%d]:  %f\n", transSeg, beta_passive[transSeg]);
//     // printf("length_passive[%d]: %f\n", transSeg, length_passive[transSeg]);
// }

// void transSegmentsPassiveSensing(){
//     for(int i=0;i<SEGNUM;i++){
//         if(i<6){
//             if(transSegmentFlag[i]){
//                 passiveSensingOneSeg(i);
//                 states_.ABL.segment[i].A = alpha_passive[i];
//                 states_.ABL.segment[i].B = beta_passive[i];
//                 states_.ABL.segment[i].L = length_passive[i];
//             }
//         }
//     }
// }

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

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

//     InitFrames();

//     State_Estimator stateEstimator;

//     ros::AsyncSpinner s(4);

//     s.start();

//     ros::Rate loop_rate(100);

//     ROS_INFO("Ready for State_Estimator_Node");

//     int printFre = 0;
//     while (ros::ok())
//     {
//         // estimateABL();

//         // test(); // given sensor information for testing
//         getABLfromLaser();

//         transSegmentsPassiveSensing();

//         stateEstimator.pub();

//         if (printFre++ > 2)
//         {
//             // printf("A[0]:%d, [1]:%d, [2]:%d, [3]:%d, [4]:%d, [5]:%d  | B[0]:%d, [1]:%d, [2]:%d, [3]:%d, [4]:%d, [5]:%d  |  L[0]:%d, [1]:%d, [2]:%d, [3]:%d, [4]:%d, [5]:%d\n",
//             //        (int)(alphaDeg[0]),
//             //        (int)(alphaDeg[1]),
//             //        (int)(alphaDeg[2]),
//             //        (int)(alphaDeg[3]),
//             //        (int)(alphaDeg[4]),
//             //        (int)(alphaDeg[5]),
//             //        (int)(betaDeg[0]),
//             //        (int)(betaDeg[1]),
//             //        (int)(betaDeg[2]),
//             //        (int)(betaDeg[3]),
//             //        (int)(betaDeg[4]),
//             //        (int)(betaDeg[5]),
//             //        (int)(lengthAbsMM[0]),
//             //        (int)(lengthAbsMM[1]),
//             //        (int)(lengthAbsMM[2]),
//             //        (int)(lengthAbsMM[3]),
//             //        (int)(lengthAbsMM[4]),
//             //        (int)(lengthAbsMM[5]));

//             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]:%.3f, [1]:%.3f, [2]:%.3f, [3]:%.3f, [4]:%.3f, [5]:%.3f\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]),
//                    (length[0]),
//                    (length[1]),
//                    (length[2]),
//                    (length[3]),
//                    (length[4]),
//                    (length[5]));
//             printFre = 0;
//         }

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

    return 0;
}


//=======================================================================
// Copyright (c)
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file sample_graph.hpp
 * @brief
 * @author Piotr Wygocki, Piotr Godlewski
 * @version 1.0
 * @date 2013-08-04
 */
#ifndef PAAL_SAMPLE_GRAPH_HPP
#define PAAL_SAMPLE_GRAPH_HPP

#include "paal/data_structures/metric/graph_metrics.hpp"
#include "paal/data_structures/metric/euclidean_metric.hpp"
#include "paal/utils/irange.hpp"

#include <boost/graph/adjacency_list.hpp>

struct sample_graphs_metrics {
    using EdgeProp = boost::property<boost::edge_weight_t, int>;
    using Graph = boost::adjacency_list<
        boost::vecS, boost::vecS, boost::undirectedS,
        boost::property<boost::vertex_color_t, int>, EdgeProp>;
    using GraphWithoutEdgeWeight = boost::adjacency_list<
        boost::vecS, boost::vecS, boost::undirectedS,
        boost::property<boost::vertex_color_t, int>, boost::no_property>;
    using ListGraph = boost::adjacency_list<
        boost::listS, boost::listS, boost::undirectedS,
        boost::property<boost::vertex_color_t, int>, EdgeProp>;
    using Edge = std::pair<int, int>;
    using GraphMT = paal::data_structures::graph_metric<Graph, int>;
    using Terminals = std::vector<int>;

    enum nodes { A, B, C, D, E, F, G, H, I, J, K, L };

    // graph small
    static Graph get_graph_small() {
        const int num_nodes = 5;
        Edge edge_array[] = { Edge(A, C), Edge(B, B), Edge(B, D),
                              Edge(B, E), Edge(C, B), Edge(C, D),
                              Edge(D, E), Edge(E, A), Edge(E, B) };
        int weights[] = { 1, 2, 1, 2, 7, 3, 1, 1, 1 };
        int num_arcs = sizeof(edge_array) / sizeof(Edge);

        Graph g(edge_array, edge_array + num_arcs, weights, num_nodes);

        return g;
    }

    static Graph get_graph_medium() {
        const int num_nodes = 9;
        Edge edge_array[] = { Edge(A, B), Edge(B, C), Edge(B, D), Edge(B, E),
                              Edge(D, E), Edge(E, F), Edge(F, G), Edge(G, H),
                              Edge(E, H), Edge(H, I) };

        int weights[] = {64, 23, 54, 63, 25, 49, 32, 15, 74, 31};
        int num_arcs = sizeof(edge_array) / sizeof(Edge);

        Graph g(edge_array, edge_array + num_arcs, weights, num_nodes);

        return g;
    }

    static ListGraph get_list_graph_medium() {
        const int num_nodes = 9;
        Edge edge_array[] = { Edge(A, B), Edge(B, C), Edge(B, D), Edge(B, E),
                              Edge(D, E), Edge(E, F), Edge(F, G), Edge(G, H),
                              Edge(E, H), Edge(H, I) };

        int weights[] = {64, 23, 54, 63, 25, 49, 32, 15, 74, 31};
        int num_arcs = sizeof(edge_array) / sizeof(Edge);

        ListGraph g(edge_array, edge_array + num_arcs, weights, num_nodes);

        return g;
    }

    static Graph get_star_medium() {
        const int num_nodes = 11;
        Edge edge_array[] = { Edge(A, B), Edge(A, C), Edge(A, D), Edge(A, E),
                              Edge(A, F), Edge(A, G), Edge(A, H), Edge(A, I),
                              Edge(A, J), Edge(A, K) };
        int weights[] = { 64, 23, 54, 63, 25, 49, 32, 15, 74, 31};
        int num_arcs = sizeof(edge_array) / sizeof(Edge);

        Graph g(edge_array, edge_array + num_arcs, weights, num_nodes);

        return g;
    }

    static Graph get_star_random(int seed, int num_nodes, int minW, int maxW) {
        std::srand(seed);

        int center = std::rand() % num_nodes;

        std::vector< std::pair<int, int> > edges;
        std::vector<int> weights(num_nodes-1);

        for (int v: paal::irange(num_nodes)) if (v != center) edges.push_back(std::make_pair(center, v));
        for (int i: paal::irange(num_nodes-1)) weights[i] = rand() % (maxW - minW + 1) + minW;

        Graph g(edges.begin(), edges.end(), weights.begin(), num_nodes);

        return g;
    }

    static GraphMT get_graph_metric_small() {
        return GraphMT(get_graph_small());
    }

    // graph steiner
    static Graph get_graph_steiner() {
        const int num_nodes = 6;
        Edge edge_array[] = { Edge(A, B), Edge(B, C), Edge(C, D),
                              Edge(D, A), Edge(A, E), Edge(B, E),
                              Edge(C, E), Edge(D, E), Edge(A, F) };
        int weights[] = { 2, 2, 2, 2, 1, 1, 1, 1, 1 };
        int num_arcs = sizeof(edge_array) / sizeof(Edge);

        Graph g(edge_array, edge_array + num_arcs, weights, num_nodes);
        auto color = get(boost::vertex_color, g);
        put(color, A, 1);
        put(color, B, 1);
        put(color, C, 1);
        put(color, D, 1);

        return g;
    }

    static Graph two_points_steiner() {
        int const  num_nodes = 2;
        Edge * edge_array = nullptr;
        int * weights = nullptr;
        Graph g(edge_array, edge_array, weights, num_nodes);
        auto color = get(boost::vertex_color, g);
        put(color, A, 1);
        put(color, B, 1);
        return g;
    }

    static Graph get_graph_steiner_multi_edges() {
        const int num_nodes = 3;
        Edge edge_array[] = { Edge(A, B), Edge(A, C), Edge(B, C), Edge(B, C),
                              Edge(B, C) };
        int weights[] = { 20, 30, 3, 5, 9 };
        int num_arcs = sizeof(edge_array) / sizeof(Edge);

        Graph g(edge_array, edge_array + num_arcs, weights, num_nodes);
        auto color = get(boost::vertex_color, g);
        put(color, A, 1);
        put(color, B, 1);
        put(color, C, 1);

        return g;
    }
    static GraphWithoutEdgeWeight get_graph_steiner_edge() {

        const int num_nodes = 2;
        Edge edge_array[] = { Edge(A, B) };
        int num_arcs = sizeof(edge_array) / sizeof(Edge);

        GraphWithoutEdgeWeight g(edge_array, edge_array + num_arcs, num_nodes);
        auto color = get(boost::vertex_color, g);
        put(color, A, 1);
        put(color, B, 1);

        return g;
    }

    static Graph get_graph_stainer_tree_cycle() {
        const int num_nodes = 5;
        Edge edge_array[] = { Edge(A, B), Edge(A, C), Edge(B, C), Edge(C, D),
                              Edge(C, E) };
        int weights[] = { 4, 2, 2, 2, 2 };
        int num_arcs = sizeof(edge_array) / sizeof(Edge);

        Graph g(edge_array, edge_array + num_arcs, weights, num_nodes);
        auto color = get(boost::vertex_color, g);
        put(color, A, 1);
        put(color, B, 1);
        put(color, D, 1);
        put(color, E, 1);

        return g;
    }

    static GraphMT get_graph_metric_steiner() {
        return GraphMT(get_graph_steiner());
    }

    static std::pair<Terminals, Terminals> get_graph_steiner_vertices() {
        Terminals terminals = { A, B, C, D }, non_terminals = { E, F };
        return std::make_pair(terminals, non_terminals);
    }

    // graph medium
    static GraphMT get_graph_metric_medium() {
        const int num_nodes = 8;
        Edge edge_array[] = { Edge(A, C), Edge(A, F), Edge(B, E), Edge(B, G),
                              Edge(B, H), Edge(C, E), Edge(C, G), Edge(C, H),
                              Edge(D, E), Edge(D, G), Edge(D, H), Edge(F, H) };
        int weights[] = { 4, 2, 1, 2, 7, 3, 1, 8, 1, 3, 4, 10 };
        int num_arcs = sizeof(edge_array) / sizeof(Edge);

        Graph g(edge_array, edge_array + num_arcs, weights, num_nodes);

        return GraphMT(g);
    }

    // graph steiner bigger
    static Graph get_graph_steiner_bigger(int p = 3, int q = 2) {
        bool b;
        int n = p + p * q;
        Graph g(n);
        for (int i = 0; i < n; i++) {
            for (int j = i + 1; j < n; j++) {
                int cost = 3;
                if (i < p && j < p) {
                    cost = 1;
                } else if ((j - p) / q == i) {
                    cost = 2;
                }
                b = add_edge(i, j, EdgeProp(cost), g).second;
                assert(b);
            }
        }
        return g;
    }

    static GraphMT get_graph_metric_steiner_bigger() {
        return GraphMT(get_graph_steiner_bigger());
    }

    static std::pair<Terminals, Terminals>
    get_graph_steiner_bigger_vertices(int p = 3, int q = 2) {
        int n = p + p * q;
        Terminals terminals, non_terminals;
        for (int i = 0; i < n; i++) {
            if (i >= p)
                terminals.push_back(i);
            else
                non_terminals.push_back(i);
        }
        return make_pair(terminals, non_terminals);
    }

    // eucildean steiner
    template <typename Points = std::vector<std::pair<int, int>>>
    static std::tuple<paal::data_structures::euclidean_metric<int>, Points,
                      Points>
    get_euclidean_steiner_sample() {
        return std::make_tuple(paal::data_structures::euclidean_metric<int>{},
                               Points{ { 0, 0 }, { 0, 2 }, { 2, 0 }, { 2, 2 } },
                               Points{ { 1, 1 } });
    }
};

#endif // PAAL_SAMPLE_GRAPH_HPP


/**
 * $Id$
 *
 * Copyright (C)
 * 2015 - $Date$
 *     Martin Wolf <ndhist@martin-wolf.org>
 *
 * This file is distributed under the BSD 2-Clause Open Source License
 * (See LICENSE file).
 *
 */
#include <boost/python.hpp>

#include <ndhist/stats/expectation.hpp>

namespace bp = boost::python;

namespace ndhist {

namespace stats {

void register_expectation()
{
    bp::def("expectation"
      , &py::expectation
      , ( bp::arg("hist")
        , bp::arg("n")=1
        , bp::arg("axis")=bp::object()
        )
      , "Calculates the n'th order expectation value along the given axis of \n"
        "the given ndhist object weighted by the sum of weights in each bin. \n"
        "It generates a projection along the given axis and then calculates  \n"
        "the n'th order expectation axis value.                              \n"
        "In statistics the n'th order expectation is defined as the          \n"
        "expectation of x^n, i.e. ``E[x^n]``, where x is the bin center axis \n"
        "value in this case.                                                 \n"
        "If ``None`` is given as axis argument (the default), the n'th order \n"
        "expectation value for all individual axes of the ndhist object is   \n"
        "calculated and returned as a tuple. But if the dimensionality of    \n"
        "the histogram is 1, a scalar value is returned.                     \n"
        "                                                                    \n"
        ".. note:: This function is only defined for ndhist objects with POD \n"
        "          type axis values AND POD type weight values.              \n"
    );
}

}// namespace stats
}// namespace ndhist


// Copyright 2015-2018 Hans Dembinski
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt
// or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/core/lightweight_test.hpp>
#include <boost/histogram/accumulators/ostream.hpp>
#include <boost/histogram/accumulators/weighted_mean.hpp>
#include <boost/histogram/weight.hpp>
#include <sstream>
#include "is_close.hpp"
#include "throw_exception.hpp"
#include "utility_str.hpp"

using namespace boost::histogram;
using namespace std::literals;

int main() {
  using m_t = accumulators::weighted_mean<double>;
  using detail::square;

  // basic interface, string conversion
  {
    // see https://en.wikipedia.org/wiki/Weighted_arithmetic_mean#Reliability_weights

    m_t a;
    BOOST_TEST_EQ(a.sum_of_weights(), 0);
    BOOST_TEST_EQ(a, m_t{});

    a(weight(0.5), 1);
    a(weight(1.0), 2);
    a(weight(0.5), 3);

    BOOST_TEST_EQ(a.sum_of_weights(), 1 + 2 * 0.5);
    BOOST_TEST_EQ(a.sum_of_weights_squared(), 1 + 2 * 0.5 * 0.5);
    const auto m = a.value();
    BOOST_TEST_IS_CLOSE(
        a.variance(),
        (0.5 * square(1 - m) + square(2 - m) + 0.5 * square(3 - m)) /
            (a.sum_of_weights() - a.sum_of_weights_squared() / a.sum_of_weights()),
        1e-3);

    BOOST_TEST_EQ(str(a), "weighted_mean(2, 2, 0.8)"s);
    BOOST_TEST_EQ(str(a, 25, false), " weighted_mean(2, 2, 0.8)"s);
    BOOST_TEST_EQ(str(a, 25, true), "weighted_mean(2, 2, 0.8) "s);
  }

  // addition of zero element
  {
    BOOST_TEST_EQ(m_t() += m_t(), m_t());
    BOOST_TEST_EQ(m_t(1, 2, 3, 4) += m_t(), m_t(1, 2, 3, 4));
    BOOST_TEST_EQ(m_t() += m_t(1, 2, 3, 4), m_t(1, 2, 3, 4));
  }

  // addition
  {
    m_t a, b, c;

    a(weight(4), 2);
    a(weight(3), 3);
    BOOST_TEST_EQ(a.sum_of_weights(), 4 + 3);
    BOOST_TEST_EQ(a.sum_of_weights_squared(), 4 * 4 + 3 * 3);
    BOOST_TEST_EQ(a.value(), (4 * 2 + 3 * 3) / 7.);
    BOOST_TEST_IS_CLOSE(a.variance(), 0.5, 1e-3);

    b(weight(2), 4);
    b(weight(1), 6);
    BOOST_TEST_EQ(b.sum_of_weights(), 3);
    BOOST_TEST_EQ(b.sum_of_weights_squared(), 1 + 2 * 2);
    BOOST_TEST_EQ(b.value(), (2 * 4 + 1 * 6) / (2. + 1.));
    BOOST_TEST_IS_CLOSE(b.variance(), 2, 1e-3);

    c(weight(4), 2);
    c(weight(3), 3);
    c(weight(2), 4);
    c(weight(1), 6);

    auto d = a;
    d += b;
    BOOST_TEST_EQ(c.sum_of_weights(), d.sum_of_weights());
    BOOST_TEST_EQ(c.sum_of_weights_squared(), d.sum_of_weights_squared());
    BOOST_TEST_EQ(c.value(), d.value());
    BOOST_TEST_IS_CLOSE(c.variance(), d.variance(), 1e-3);
  }

  // using weights * 2 compared to adding weighted samples twice must
  // - give same for sum_of_weights and mean
  // - give twice sum_of_weights_squared
  // - give half effective count
  // - variance is complicated, but larger
  {
    m_t a, b;

    for (int i = 0; i < 2; ++i) {
      a(weight(0.5), 1);
      a(weight(1.0), 2);
      a(weight(0.5), 3);
    }

    b(weight(1), 1);
    b(weight(2), 2);
    b(weight(1), 3);

    BOOST_TEST_EQ(a.sum_of_weights(), b.sum_of_weights());
    BOOST_TEST_EQ(2 * a.sum_of_weights_squared(), b.sum_of_weights_squared());
    BOOST_TEST_EQ(a.value(), b.value());
    BOOST_TEST_LT(a.variance(), b.variance());
  }

  return boost::report_errors();
}