#include<iostream>
#include <string>
#include <ros/ros.h>
#include <ros/time.h>
#include <sensor_msgs/JointState.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Bool.h>
#include <robot_state_publisher/robot_state_publisher.h>
#include <cmath>
#include <math.h>
#include <eigen3/Eigen/Dense>
#include <Eigen/Dense>
#include "gazebo_msgs/LinkStates.h"
#include "gazebo_msgs/LinkState.h"

#include "geometry_msgs/Pose.h"
#include "geometry_msgs/Twist.h"

#include "inverse_kinematics.h"

using namespace std;
Eigen::Matrix<double, 3, 1> q_i_LF;
Eigen::Matrix<double, 3, 1> q_i_LH;
Eigen::Matrix<double, 3, 1> q_i_RF;
Eigen::Matrix<double, 3, 1> q_i_RH;

Eigen::Matrix<double, 3, 1> pos_foot_LF;
Eigen::Matrix<double, 3, 1> pos_foot_LH;
Eigen::Matrix<double, 3, 1> pos_foot_RF;
Eigen::Matrix<double, 3, 1> pos_foot_RH;

Eigen::Matrix<double, 6, 1> pos_base;

bool friction_cone;
bool friction_cone_dietro;
bool friction_dietro;
bool friction_dietro_due;
bool scivolo;
bool scivolo_dietro;
bool scivolo_due;
bool scivolo_dietro_due;

bool fine;
bool endd;
bool freeze;
double delta_x_avanti;
double conta_friction;

void get_scivolo(const std_msgs::BoolConstPtr& msg)
{
 scivolo = msg->data;
}

void get_delta_x_avanti(const std_msgs::Float64ConstPtr& msg)
{
 delta_x_avanti = msg->data;
}

void get_conta_friction(const std_msgs::Float64ConstPtr& msg)
{
 conta_friction= msg->data;
}

void get_scivolo_dietro(const std_msgs::BoolConstPtr& msg)
{
 scivolo_dietro = msg->data;
}

void get_scivolo_due(const std_msgs::BoolConstPtr& msg)
{
 scivolo_due = msg->data;
}

void get_scivolo_dietro_due(const std_msgs::BoolConstPtr& msg)
{
 scivolo_dietro_due = msg->data;
}

void get_freeze(const std_msgs::BoolConstPtr& msg)
{
 freeze = msg->data;
}

void get_friction_cone(const std_msgs::BoolConstPtr& msg)
{
 friction_cone = msg->data;
}

void get_friction_cone_dietro(const std_msgs::BoolConstPtr& msg)
{
 friction_cone_dietro = msg->data;
}

void get_friction_dietro(const std_msgs::BoolConstPtr& msg)
{
 friction_dietro = msg->data;
}

void get_friction_dietro_due(const std_msgs::BoolConstPtr& msg)
{
 friction_dietro_due = msg->data;
}

void get_pos_base(const geometry_msgs::TwistConstPtr& msg)
{
        pos_base(0) = msg->linear.x;
        pos_base(1) = msg->linear.y;
        pos_base(2) = msg->linear.z;

        pos_base(3)= msg->angular.x;
        pos_base(4)= msg->angular.y;
        pos_base(5)= msg->angular.z;

}

void get_q_i_L(const geometry_msgs::TwistConstPtr& msg)
{
        q_i_LF(0) = msg->linear.x;
        q_i_LF(1) = msg->linear.y;
        q_i_LF(2) = msg->linear.z;

        q_i_LH(0) = msg->angular.x;
        q_i_LH(1) = msg->angular.y;
        q_i_LH(2) = msg->angular.z;

}

void get_q_i_R(const geometry_msgs::TwistConstPtr& msg)
{
        q_i_RF(0) = msg->linear.x;
        q_i_RF(1) = msg->linear.y;
        q_i_RF(2) = msg->linear.z;

        q_i_RH(0) = msg->angular.x;
        q_i_RH(1) = msg->angular.y;
        q_i_RH(2) = msg->angular.z;

}

void get_L_foot_pos(const geometry_msgs::TwistConstPtr& msg)
{
      pos_foot_LF(0) = msg->linear.x;
      pos_foot_LF(1) = msg->linear.y;
      pos_foot_LF(2) = msg->linear.z;

      pos_foot_LH(0) = msg->angular.x;
      pos_foot_LH(1) = msg->angular.y;
      pos_foot_LH(2) = msg->angular.z;
}

void get_R_foot_pos(const geometry_msgs::TwistConstPtr& msg)
{
      pos_foot_RF(0) = msg->linear.x;
      pos_foot_RF(1) = msg->linear.y;
      pos_foot_RF(2) = msg->linear.z;

      pos_foot_RH(0) = msg->angular.x;
      pos_foot_RH(1) = msg->angular.y;
      pos_foot_RH(2) = msg->angular.z;
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "set_command"); //The name of the node
  ros::NodeHandle n;
  ros::Rate loop_rate(100);

  ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("/anymal/floating_base_controller/command", 1);
  ros::Publisher base_pub = n.advertise<sensor_msgs::JointState>("/anymal/floating_base_controller/command_base", 1);

  ros::Subscriber q_i_L_sub = n.subscribe<geometry_msgs::Twist>("/anymal/floating_base_controller/q_i_L_pub", 1, get_q_i_L);
  ros::Subscriber q_i_R_sub = n.subscribe<geometry_msgs::Twist>("/anymal/floating_base_controller/q_i_R_pub", 1, get_q_i_R);

  ros::Subscriber pos_foot_L_sub = n.subscribe<geometry_msgs::Twist>("/anymal/floating_base_controller/pos_foot_L_pub", 1, get_L_foot_pos);
  ros::Subscriber pos_foot_R_sub = n.subscribe<geometry_msgs::Twist>("/anymal/floating_base_controller/pos_foot_R_pub", 1, get_R_foot_pos);

  ros::Subscriber pos_base_sub = n.subscribe<geometry_msgs::Twist>("/anymal/floating_base_controller/pos_base_pub", 1, get_pos_base);

  ros::Subscriber freeze_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/freeze_pub", 1, get_freeze);
  ros::Subscriber friction_cone_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/friction_cone_pub", 1, get_friction_cone);
  ros::Subscriber friction_cone_dietro_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/friction_cone_dietro_pub", 1, get_friction_cone_dietro);
  ros::Subscriber friction_dietro_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/friction_dietro_pub", 1, get_friction_dietro);
  ros::Subscriber friction_dietro_due_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/friction_dietro_due_pub", 1, get_friction_dietro_due);
  ros::Subscriber scivolo_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/scivolo_pub", 1, get_scivolo);
  ros::Subscriber scivolo_due_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/scivolo_due_pub", 1, get_scivolo_due);
  ros::Subscriber scivolo_dietro_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/scivolo_dietro_pub", 1, get_scivolo);
  ros::Subscriber scivolo_dietro_due_sub = n.subscribe<std_msgs::Bool>("/anymal/floating_base_controller/scivolo_dietro_due_pub", 1, get_scivolo_dietro_due);
  ros::Subscriber delta_x_avanti_sub = n.subscribe<std_msgs::Float64>("/anymal/floating_base_controller/delta_x_avanti_pub", 1000, get_delta_x_avanti);
  ros::Subscriber conta_friction_sub = n.subscribe<std_msgs::Float64>("/anymal/floating_base_controller/conta_friction_pub", 1000, get_conta_friction);

  ros::Publisher c_switch_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_c_switch_set_command", 1);
  ros::Publisher fine_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_fine_set_command", 1);
  ros::Publisher end_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_end_set_command", 1);
  ros::Publisher scivolo_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_scivolo", 1);
  ros::Publisher scivolo_due_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_scivolo_due", 1);
  ros::Publisher scivolo_dietro_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_scivolo_dietro", 1);
  ros::Publisher scivolo_dietro_due_pub = n.advertise<std_msgs::Bool>("/anymal/floating_base_controller/get_scivolo_dietro_due", 1);
  ros::Publisher conta_friction_pub = n.advertise<std_msgs::Float64>("/anymal/floating_base_controller/conta_friction_pub", 1000);
  ros::Publisher delta_x_avanti_pub = n.advertise<std_msgs::Float64>("/anymal/floating_base_controller/delta_x_avanti_pub", 1000);

  sensor_msgs::JointState joint_state;
  sensor_msgs::JointState base_state;

  double q1_time_LF; //= q_i_LF(0);
  double q2_time_LF; //= q_i_LF(1);
  double q3_time_LF; //= q_i_LF(2);

  double q1_f_LF;
  double q2_f_LF;
  double q3_f_LF;

  double q1_time_LH;
  double q2_time_LH;
  double q3_time_LH;

  double q1_f_LH;
  double q2_f_LH;
  double q3_f_LH;

  double q1_time_RF;
  double q2_time_RF;
  double q3_time_RF;

  double q1_f_RF;
  double q2_f_RF;
  double q3_f_RF;

  double q1_time_RH;
  double q2_time_RH;
  double q3_time_RH;

  double q1_f_RH;
  double q2_f_RH;
  double q3_f_RH;

double delta_x_wp1_L = 0.085;
double delta_x_wp2_L = 0.085;
double delta_x_wp1_R = 0.085;
double delta_x_wp2_R = 0.085;

  double delta_x_wp1_Ll = 0.074;
  double delta_y_wp1_L = 0.0;
  double delta_h_wp1_L = 0.025;

  double delta_x_wp2_Ll = 0.074;
  double delta_y_wp2_L = 0.0;
  double delta_h_wp2_L = -0.025;

  double delta_x_wp1_LF = 0.0742;
  double delta_y_wp1_LF = 0.0;
  double delta_h_wp1_LF = 0.025;

  double delta_x_wp2_LF = 0.0742;
  double delta_y_wp2_LF = 0.0;
  double delta_h_wp2_LF = -0.025;

  double delta_x_wp1_LH = 0.0755; //0.0755;
  double delta_y_wp1_LH = 0.0;
  double delta_h_wp1_LH = 0.025;

  double delta_x_wp2_LH = 0.0755; //0.0755;
  double delta_y_wp2_LH = 0.0;
  double delta_h_wp2_LH = -0.025;

  double delta_x_wp1_RF = 0.076;
  double delta_y_wp1_RF = 0.0;
  double delta_h_wp1_RF = 0.025;

  double delta_x_wp2_RF = 0.076;
  double delta_y_wp2_RF = 0.0;
  double delta_h_wp2_RF = -0.025;

  double delta_x_wp1_RH = 0.08;
  double delta_y_wp1_RH = 0.0;
  double delta_h_wp1_RH = 0.025;

  double delta_x_wp2_RH = 0.08;
  double delta_y_wp2_RH = 0.0;
  double delta_h_wp2_RH = -0.025;

  double delta_y_wp1_RF_fd = -0.004;
  double delta_y_wp2_RF_fd = -0.004;

   double delta_x_wp1_RFF = 0.08;//0.04; //0.035;
   double delta_y_wp1_RFF = -0.0025;//2;//0.006;
   double delta_h_wp1_RFF = 0.025;

   double delta_x_wp2_RFF = 0.08; //0.04; // 0.035
   double delta_y_wp2_RFF = -0.0025; //2;//0.006;
   double delta_h_wp2_RFF = -0.025;

   double delta_x_wp1_RHH = 0.076;
   double delta_y_wp1_RHH = -0.008; //-0.02;
   double delta_h_wp1_RHH = 0.025;

   double delta_x_wp2_RHH = 0.076;
   double delta_y_wp2_RHH = -0.008; //-0.02;
   double delta_h_wp2_RHH = -0.025;

   double delta_x_wp1_LFF = 0.075;
   double delta_y_wp1_LFF = 0.0025; //0.008;//0.008;//0.006;//0.02;
   double delta_h_wp1_LFF = 0.025;

   double delta_x_wp2_LFF = 0.075;
   double delta_y_wp2_LFF = 0.0025; //0.008; //0.008; //0.006; //0.02;
   double delta_h_wp2_LFF = -0.025;

   double delta_x_wp1_LHH = 0.075; //0.072; //0.075;
   double delta_y_wp1_LHH = 0.0005;//0.002 -0.004; //-0.008; //-0.004;//-0.003; //0.0;//-0.03; //-0.03;
   double delta_h_wp1_LHH = 0.025;

   double delta_x_wp2_LHH = 0.075; //0.072; //0.075;
   double delta_y_wp2_LHH = 0.0005; //0.002 -0.004; //-0.008; //-0.004; //- 0.003; //0.0; //-0.03; //-0.03;
   double delta_h_wp2_LHH = -0.025;

  double x_wp_1;
  double y_wp_1;
  double z_wp_1;
  double x_wp_2;
  double y_wp_2;
  double z_wp_2;

  double conta = 0;

  double delta_x = 0.078;// - delta_x_avanti;
  double delta_xx = 0.078; // - delta_x_avanti;

  double x_wp_dxx = 0.075;
  double x_wp_dx = 0.09;
  double y_wp_dx = 0.08;
  double z_wp_dx = 0.54;
  double roll_wp_dx = -0.03;
  double pitch_wp_dx = -0.01;
  double yaw_wp_dx = 0.0;

  double x_wp_sxx = 0.045;
  double x_wp_sx = 0.06;
  double y_wp_sx = - 0.08;
  double z_wp_sx = 0.54;
  double roll_wp_sx = 0.02;
  double pitch_wp_sx = -0.01;
  double yaw_wp_sx = 0.0;

  double x_time;
  double y_time;
  double z_time;
  double roll_time;
  double pitch_time;
  double yaw_time;

  double pitch = 0.2;

  double time_f = 0.5;
  double time_f_base = 2;

  double contatore = 0;

  double t;

  bool leg_up = 1;
  bool c_switch = 0;

  bool base_dx = 0;

 std_msgs::Bool c_switch_msg;
 std_msgs::Bool fine_msg;  // c_switch
 std_msgs::Bool end_msg;
 std_msgs::Bool scivolo_msg;

  Eigen::Matrix<double, 4, 1> move_leg;
  move_leg << 0 ,1 , 0, 0;

  while (ros::ok())
    {

      t = ros::Time::now().toSec();

      if(t > 2 && t < 2.1)
      {
        c_switch = 1;
      }

      //if (c_switch == 1 && t < 95)

      if (c_switch == 1 && scivolo == 0 && scivolo_due == 0 && scivolo_dietro == 0 && scivolo_dietro_due == 0)
      {
           if ( move_leg(1) == 1)
        {
          q1_time_LF = q_i_LF(0);
          q2_time_LF = q_i_LF(1);
          q3_time_LF = q_i_LF(2);

          q1_time_LH = q_i_LH(0);
          q2_time_LH = q_i_LH(1);
          q3_time_LH = q_i_LH(2);

          q1_time_RF = q_i_RF(0);
          q2_time_RF = q_i_RF(1);
          q3_time_RF = q_i_RF(2);

          q1_time_RH = q_i_RH(0);
          q2_time_RH = q_i_RH(1);
          q3_time_RH = q_i_RH(2);

          if(friction_cone == 0){
          if(conta == 0 || conta == 1){
            scivolo = 0;
            scivolo_due = 0;
            scivolo_dietro = 0;
            scivolo_dietro_due = 0;

          if(leg_up == 1)
          {

            x_wp_1 = pos_foot_LH(0) + delta_x_wp1_Ll;
            y_wp_1 = pos_foot_LH(1) + delta_y_wp1_L;
            z_wp_1 = pos_foot_LH(2) + delta_h_wp1_L;

            Inverse_Kinematics_LH(x_wp_1,y_wp_1,z_wp_1,q1_f_LH,q2_f_LH,q3_f_LH);

            interpolation(q_i_LH(0), q_i_LH(1), q_i_LH(2), q1_f_LH, q2_f_LH, q3_f_LH, q1_time_LH, q2_time_LH, q3_time_LH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_LH(0) = q1_f_LH;
              q_i_LH(1) = q2_f_LH;
              q_i_LH(2) = q3_f_LH;
              contatore = 0;
              leg_up = 0;

            }
          }

          else
          {

            x_wp_2 = x_wp_1 + delta_x_wp2_Ll;
            y_wp_2 = y_wp_1 + delta_y_wp2_L;
            z_wp_2 = z_wp_1 + delta_h_wp2_L;

            Inverse_Kinematics_LH(x_wp_2,y_wp_2,z_wp_2,q1_f_LH,q2_f_LH,q3_f_LH);

            interpolation(q_i_LH(0), q_i_LH(1), q_i_LH(2), q1_f_LH, q2_f_LH, q3_f_LH, q1_time_LH, q2_time_LH, q3_time_LH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 1,0,0,0;
              conta++;

            }
          }

        }

        else{

           scivolo = 0;
            scivolo_due = 0;
            scivolo_dietro = 0;
            scivolo_dietro_due = 0;

          if(leg_up == 1)
          {

            x_wp_1 = pos_foot_LH(0) + delta_x_wp1_LH;
            y_wp_1 = pos_foot_LH(1) + delta_y_wp1_LH;
            z_wp_1 = pos_foot_LH(2) + delta_h_wp1_LH;

            Inverse_Kinematics_LH(x_wp_1,y_wp_1,z_wp_1,q1_f_LH,q2_f_LH,q3_f_LH);

            interpolation(q_i_LH(0), q_i_LH(1), q_i_LH(2), q1_f_LH, q2_f_LH, q3_f_LH, q1_time_LH, q2_time_LH, q3_time_LH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_LH(0) = q1_f_LH;
              q_i_LH(1) = q2_f_LH;
              q_i_LH(2) = q3_f_LH;
              contatore = 0;
              leg_up = 0;

            }
          }

          else
          {

            x_wp_2 = x_wp_1 + delta_x_wp2_LH;
            y_wp_2 = y_wp_1 + delta_y_wp2_LH;
            z_wp_2 = z_wp_1 + delta_h_wp2_LH;

            Inverse_Kinematics_LH(x_wp_2,y_wp_2,z_wp_2,q1_f_LH,q2_f_LH,q3_f_LH);

            interpolation(q_i_LH(0), q_i_LH(1), q_i_LH(2), q1_f_LH, q2_f_LH, q3_f_LH, q1_time_LH, q2_time_LH, q3_time_LH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 1,0,0,0;

            }
          }

        }
          }

        else{
          if( friction_cone == 1){
          if(leg_up == 1)
          {
            friction_cone = 1;
            x_wp_1 = pos_foot_LH(0) + delta_x_wp1_LHH;//delta_x_wp1_LH_fd;
            y_wp_1 = pos_foot_LH(1) + delta_y_wp1_LH;
            z_wp_1 = pos_foot_LH(2) + delta_h_wp1_LHH;

            Inverse_Kinematics_LH(x_wp_1,y_wp_1,z_wp_1,q1_f_LH,q2_f_LH,q3_f_LH);

            interpolation(q_i_LH(0), q_i_LH(1), q_i_LH(2), q1_f_LH, q2_f_LH, q3_f_LH, q1_time_LH, q2_time_LH, q3_time_LH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_LH(0) = q1_f_LH;
              q_i_LH(1) = q2_f_LH;
              q_i_LH(2) = q3_f_LH;
              contatore = 0;
              leg_up = 0;
              friction_cone = 1;

            }
          }

          else
          {
            friction_cone = 1;
            x_wp_2 = x_wp_1 + delta_x_wp2_LHH;// delta_x_wp2_LH_fd;
            y_wp_2 = y_wp_1 + delta_y_wp2_LH;
            z_wp_2 = z_wp_1 + delta_h_wp2_LHH;
            //cout<<"x_wp_sx_LL"<<endl<<x_wp_2<<endl;

            Inverse_Kinematics_LH(x_wp_2,y_wp_2,z_wp_2,q1_f_LH,q2_f_LH,q3_f_LH);

            interpolation(q_i_LH(0), q_i_LH(1), q_i_LH(2), q1_f_LH, q2_f_LH, q3_f_LH, q1_time_LH, q2_time_LH, q3_time_LH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 1,0,0,0;
              friction_cone = 1;

           }
          }
         }
        }
        }

        else if ( move_leg(0) == 1)
        {
          if(friction_cone == 0){
          cout<<"normale"<<endl;
          scivolo = 0;
          scivolo_due = 0;
          scivolo_dietro = 0;
          scivolo_dietro_due = 0;
          if(leg_up == 1)
          {

            x_wp_1 = pos_foot_LF(0) + delta_x_wp1_LF;
            y_wp_1 = pos_foot_LF(1) + delta_y_wp1_LF;
            z_wp_1 = pos_foot_LF(2) + delta_h_wp1_LF;

            Inverse_Kinematics_LF(x_wp_1,y_wp_1,z_wp_1,q1_f_LF,q2_f_LF,q3_f_LF);

            interpolation(q_i_LF(0), q_i_LF(1), q_i_LF(2), q1_f_LF, q2_f_LF, q3_f_LF, q1_time_LF, q2_time_LF, q3_time_LF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_LF(0) = q1_f_LF;
              q_i_LF(1) = q2_f_LF;
              q_i_LF(2) = q3_f_LF;
              contatore = 0;
              leg_up = 0;

            }

          }

          else{

            x_wp_2 = x_wp_1 + delta_x_wp2_LF;
            y_wp_2 = y_wp_1 + delta_y_wp2_LF;
            z_wp_2 = z_wp_1 + delta_h_wp2_LF;

            Inverse_Kinematics_LF(x_wp_2,y_wp_2,z_wp_2,q1_f_LF,q2_f_LF,q3_f_LF);

            interpolation(q_i_LF(0), q_i_LF(1), q_i_LF(2), q1_f_LF, q2_f_LF, q3_f_LF, q1_time_LF, q2_time_LF, q3_time_LF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
                contatore = 0;
                leg_up = 1;
                move_leg << 0,0,0,1;
              c_switch = 0;
              base_dx = 1;

              x_time = x_wp_sx;
              y_time = y_wp_sx;
              z_time = z_wp_sx;
              roll_time = roll_wp_sx;
              pitch_time = pitch_wp_sx;
              yaw_time = yaw_wp_sx;

              delta_x_wp1_L = delta_x_wp1_R;
              delta_x_wp2_L = delta_x_wp2_R;

            }

          }
        }
        else{
          if(leg_up == 1)
          {
            friction_cone = 1;

            x_wp_1 = pos_foot_LF(0) + delta_x_wp1_LF;
            y_wp_1 = pos_foot_LF(1) + delta_y_wp1_LFF;
            z_wp_1 = pos_foot_LF(2) + delta_h_wp1_LFF;

            Inverse_Kinematics_LF(x_wp_1,y_wp_1,z_wp_1,q1_f_LF,q2_f_LF,q3_f_LF);

            interpolation(q_i_LF(0), q_i_LF(1), q_i_LF(2), q1_f_LF, q2_f_LF, q3_f_LF, q1_time_LF, q2_time_LF, q3_time_LF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_LF(0) = q1_f_LF;
              q_i_LF(1) = q2_f_LF;
              q_i_LF(2) = q3_f_LF;
              contatore = 0;
              leg_up = 0;
              friction_cone = 1;

            }

          }

          else{

            friction_cone = 1;
            x_wp_2 = x_wp_1 + delta_x_wp2_LF;
            y_wp_2 = y_wp_1 + delta_y_wp2_LFF;
            z_wp_2 = z_wp_1 + delta_h_wp2_LFF;

            Inverse_Kinematics_LF(x_wp_2,y_wp_2,z_wp_2,q1_f_LF,q2_f_LF,q3_f_LF);

            interpolation(q_i_LF(0), q_i_LF(1), q_i_LF(2), q1_f_LF, q2_f_LF, q3_f_LF, q1_time_LF, q2_time_LF, q3_time_LF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
                contatore = 0;
                leg_up = 1;
                move_leg << 0,0,0,1;
                c_switch = 0;
                base_dx = 1;
                friction_cone = 1;

                x_time = x_wp_sx;
                y_time = y_wp_sx;
                z_time = z_wp_sx;
                roll_time = roll_wp_sx;
                pitch_time = pitch_wp_sx;
                yaw_time = yaw_wp_sx;

                delta_x_wp1_L = delta_x_wp1_R;
                delta_x_wp2_L = delta_x_wp2_R;

            }

          }
        }

        }

     else if(move_leg(3) == 1)
        {
          if(friction_cone == 0){
            scivolo = 0;
            scivolo_due = 0;
            scivolo_dietro = 0;
            scivolo_dietro_due = 0;

          if(leg_up == 1)
          {
            x_wp_1 = pos_foot_RH(0) + delta_x_wp1_RH;
            y_wp_1 = pos_foot_RH(1) + delta_y_wp1_RH;
            z_wp_1 = pos_foot_RH(2) + delta_h_wp1_RH;

            Inverse_Kinematics_RH(x_wp_1,y_wp_1,z_wp_1,q1_f_RH,q2_f_RH,q3_f_RH);

            interpolation(q_i_RH(0), q_i_RH(1), q_i_RH(2), q1_f_RH, q2_f_RH, q3_f_RH, q1_time_RH, q2_time_RH, q3_time_RH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_RH(0) = q1_f_RH;
              q_i_RH(1) = q2_f_RH;
              q_i_RH(2) = q3_f_RH;
              contatore = 0;
              leg_up = 0;

            }
          }

          else
          {

            x_wp_2 = x_wp_1 + delta_x_wp2_RH;
            y_wp_2 = y_wp_1 + delta_y_wp2_RH;
            z_wp_2 = z_wp_1 + delta_h_wp2_RH;

            Inverse_Kinematics_RH(x_wp_2,y_wp_2,z_wp_2,q1_f_RH,q2_f_RH,q3_f_RH);

            interpolation(q_i_RH(0), q_i_RH(1), q_i_RH(2), q1_f_RH, q2_f_RH, q3_f_RH, q1_time_RH, q2_time_RH, q3_time_RH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 0,0,1,0;

            }
          }
        }

        else{
          if(friction_cone == 1){

          if(leg_up == 1)
          {
            friction_cone = 1;
            x_wp_1 = pos_foot_RH(0) + delta_x_wp1_RHH;
            y_wp_1 = pos_foot_RH(1) + delta_y_wp1_RH;
            z_wp_1 = pos_foot_RH(2) + delta_h_wp1_RHH;

            Inverse_Kinematics_RH(x_wp_1,y_wp_1,z_wp_1,q1_f_RH,q2_f_RH,q3_f_RH);

            interpolation(q_i_RH(0), q_i_RH(1), q_i_RH(2), q1_f_RH, q2_f_RH, q3_f_RH, q1_time_RH, q2_time_RH, q3_time_RH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_RH(0) = q1_f_RH;
              q_i_RH(1) = q2_f_RH;
              q_i_RH(2) = q3_f_RH;
              contatore = 0;
              leg_up = 0;

             friction_cone = 1;

            }
          }

          else
          {
            friction_cone = 1;
            x_wp_2 = x_wp_1 + delta_x_wp2_RHH; //delta_x_wp2_RH_fd;
            y_wp_2 = y_wp_1 + delta_y_wp2_RH;
            z_wp_2 = z_wp_1 + delta_h_wp2_RHH;

            Inverse_Kinematics_RH(x_wp_2,y_wp_2,z_wp_2,q1_f_RH,q2_f_RH,q3_f_RH);

            interpolation(q_i_RH(0), q_i_RH(1), q_i_RH(2), q1_f_RH, q2_f_RH, q3_f_RH, q1_time_RH, q2_time_RH, q3_time_RH, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 0,0,1,0;
               friction_cone = 1;

            }
          }
        }

        }
        }

       else if(move_leg(2) == 1)
        {
          scivolo = 0;
          scivolo_due = 0;
          scivolo_dietro = 0;
          scivolo_dietro_due = 0;
          if(friction_cone == 0){
          if(leg_up == 1)
          {

            x_wp_1 = pos_foot_RF(0) + delta_x_wp1_RF;
            y_wp_1 = pos_foot_RF(1) + delta_y_wp1_RF;
            z_wp_1 = pos_foot_RF(2) + delta_h_wp1_RF;

            Inverse_Kinematics_RF(x_wp_1,y_wp_1,z_wp_1,q1_f_RF,q2_f_RF,q3_f_RF);

            interpolation(q_i_RF(0), q_i_RF(1), q_i_RF(2), q1_f_RF, q2_f_RF, q3_f_RF, q1_time_RF, q2_time_RF, q3_time_RF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_RF(0) = q1_f_RF;
              q_i_RF(1) = q2_f_RF;
              q_i_RF(2) = q3_f_RF;
              contatore = 0;
              leg_up = 0;

          }
          }

          else
          {

            x_wp_2 = x_wp_1 + delta_x_wp2_RF;
            y_wp_2 = y_wp_1 + delta_y_wp2_RF;
            z_wp_2 = z_wp_1 + delta_h_wp2_RF;

            Inverse_Kinematics_RF(x_wp_2,y_wp_2,z_wp_2,q1_f_RF,q2_f_RF,q3_f_RF);

            interpolation(q_i_RF(0), q_i_RF(1), q_i_RF(2), q1_f_RF, q2_f_RF, q3_f_RF, q1_time_RF, q2_time_RF, q3_time_RF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 0,1,0,0;
              c_switch = 0;
              base_dx = 0;
              //endd = 0;
              conta_friction = 0;

               x_time = x_wp_dx; // * cos(theta) + z_wp_dx * sin(theta); ;
              y_time = y_wp_dx;
              z_time = z_wp_dx; // * cos(theta) - x_wp_dx * sin(theta);;
              roll_time = roll_wp_dx;
              pitch_time = pitch_wp_dx;
              yaw_time = yaw_wp_dx;

              delta_x_wp1_L = delta_x_wp1_R;
              delta_x_wp2_L = delta_x_wp2_R;
            }
          }
        }
        else{
          if(friction_cone == 1){
           if(leg_up == 1)
          {
            friction_cone = 1;
            scivolo = 0;
          scivolo_due = 0;
          scivolo_dietro = 0;
          scivolo_dietro_due = 0;

            x_wp_1 = pos_foot_RF(0) + delta_x_wp1_RF;
            y_wp_1 = pos_foot_RF(1) + delta_y_wp1_RFF;
            z_wp_1 = pos_foot_RF(2) + delta_h_wp1_RFF;

            Inverse_Kinematics_RF(x_wp_1,y_wp_1,z_wp_1,q1_f_RF,q2_f_RF,q3_f_RF);

            interpolation(q_i_RF(0), q_i_RF(1), q_i_RF(2), q1_f_RF, q2_f_RF, q3_f_RF, q1_time_RF, q2_time_RF, q3_time_RF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {

              q_i_RF(0) = q1_f_RF;
              q_i_RF(1) = q2_f_RF;
              q_i_RF(2) = q3_f_RF;
              contatore = 0;
              leg_up = 0;

          }
          }

         else
          {

            x_wp_2 = x_wp_1 + delta_x_wp2_RF;
            y_wp_2 = y_wp_1 + delta_y_wp2_RFF;
            z_wp_2 = z_wp_1 + delta_h_wp2_RFF;

            Inverse_Kinematics_RF(x_wp_2,y_wp_2,z_wp_2,q1_f_RF,q2_f_RF,q3_f_RF);

            interpolation(q_i_RF(0), q_i_RF(1), q_i_RF(2), q1_f_RF, q2_f_RF, q3_f_RF, q1_time_RF, q2_time_RF, q3_time_RF, contatore, time_f);

            contatore = contatore + 0.01;

            if (contatore > time_f + 0.01)
            {
              contatore = 0;
              leg_up = 1;
              move_leg << 0,1,0,0;
              c_switch = 0;
              base_dx = 0;
              endd = 1;
              conta_friction++;
              //friction_cone = 0;

               x_time = x_wp_dx; // * cos(theta) + z_wp_dx * sin(theta); ;
              y_time = y_wp_dx;
              z_time = z_wp_dx; // * cos(theta) - x_wp_dx * sin(theta);;
              roll_time = roll_wp_dx;
              pitch_time = pitch_wp_dx;
              yaw_time = yaw_wp_dx;

              delta_x_wp1_L = delta_x_wp1_R;
              delta_x_wp2_L = delta_x_wp2_R;
            }
          }
        }

        }
      }
      }

      else if(c_switch == 0 && t > 2.1 && base_dx == 1)
      {

        move_base(x_wp_sx,y_wp_sx,z_wp_sx,roll_wp_sx,pitch_wp_sx,yaw_wp_sx,x_wp_dx,y_wp_dx,z_wp_dx,roll_wp_dx,pitch_wp_dx,yaw_wp_dx,x_time, y_time, z_time, roll_time, pitch_time, yaw_time,contatore,time_f_base);

        contatore = contatore + 0.01;

        if(contatore > time_f_base + 0.01)
        {
            contatore = 0;
            c_switch = 1;

            x_wp_sx = x_wp_dx + delta_x;

            q1_time_LF = q_i_LF(0);
            q2_time_LF = q_i_LF(1);
            q3_time_LF = q_i_LF(2);

            q1_time_LH = q_i_LH(0);
            q2_time_LH = q_i_LH(1);
            q3_time_LH = q_i_LH(2);

            q1_time_RF = q_i_RF(0);
            q2_time_RF = q_i_RF(1);
            q3_time_RF = q_i_RF(2);

            q1_time_RH = q_i_RH(0);
            q2_time_RH = q_i_RH(1);
            q3_time_RH = q_i_RH(2);

        }
      }

      else if(c_switch == 0 && t > 2.1 && base_dx == 0)
      {

        move_base(x_wp_dx,y_wp_dx,z_wp_dx,roll_wp_dx,pitch_wp_dx,yaw_wp_dx,x_wp_sx,y_wp_sx,z_wp_sx,roll_wp_sx,pitch_wp_sx,yaw_wp_sx,x_time, y_time, z_time, roll_time, pitch_time, yaw_time,contatore,time_f_base);

        contatore = contatore + 0.01;

        if(contatore > time_f_base + 0.01)
        {
            contatore = 0;
            c_switch = 1;

            x_wp_dx = x_wp_sx + delta_x;

            q1_time_LF = q_i_LF(0);
            q2_time_LF = q_i_LF(1);
            q3_time_LF = q_i_LF(2);

            q1_time_LH = q_i_LH(0);
            q2_time_LH = q_i_LH(1);
            q3_time_LH = q_i_LH(2);

            q1_time_RF = q_i_RF(0);
            q2_time_RF = q_i_RF(1);
            q3_time_RF = q_i_RF(2);

            q1_time_RH = q_i_RH(0);
            q2_time_RH = q_i_RH(1);
            q3_time_RH = q_i_RH(2);

        }
      }

      base_state.name.resize(6);
      base_state.position.resize(6);
      base_state.velocity.resize(6);
      base_state.effort.resize(6);

      joint_state.name.resize(12);
      joint_state.position.resize(12);
      joint_state.velocity.resize(12);
      joint_state.effort.resize(12);

      joint_state.name[0]="LF_HAA";
      joint_state.position[0] = q1_time_LF;
      joint_state.velocity[0] = 0.0;
      joint_state.effort[0] = 0.0;

      joint_state.name[1]="LF_HFE";
      joint_state.position[1] = q2_time_LF;
      joint_state.velocity[1] = 0.0;
      joint_state.effort[1] = 0.0;

      joint_state.name[2]="LF_KFE";
      joint_state.position[2] = q3_time_LF;
      joint_state.velocity[2] = 0.0;
      joint_state.effort[2] = 0.0;

      joint_state.name[3]="LH_HAA";
      joint_state.position[3] = q1_time_LH;
      joint_state.effort[3] = 0.0;

      joint_state.name[4]="LH_HFE";
      joint_state.position[4] = q2_time_LH;
      joint_state.velocity[4] = 0.0;
      joint_state.effort[4] = 0.0;

      joint_state.name[5]="LH_KFE";
      joint_state.position[5] = q3_time_LH;
      joint_state.velocity[5] = 0.0;
      joint_state.effort[5] = 0.0;

      joint_state.name[6]="RF_HAA";
      joint_state.position[6] = q1_time_RF;
      joint_state.velocity[6] = 0.0;
      joint_state.effort[6] = 0.0;

      joint_state.name[7]="RF_HFE";
      joint_state.position[7] = q2_time_RF;
      joint_state.velocity[7] = 0.0;
      joint_state.effort[7] = 0.0;

      joint_state.name[8]="RF_KFE";
      joint_state.position[8] = q3_time_RF;
      joint_state.effort[8] = 0.0;

      joint_state.name[9]="RH_HAA";
      joint_state.position[9] = q1_time_RH;
      joint_state.velocity[9] = 0.0;
      joint_state.effort[9] = 0.0;

      joint_state.name[10]="RH_HFE";
      joint_state.position[10] = q2_time_RH;
      joint_state.velocity[10] = 0.0;
      joint_state.effort[10] = 0.0;

      joint_state.name[11]="RH_KFE";
      joint_state.position[11] =  q3_time_RH;
      joint_state.velocity[11] = 0.0;
      joint_state.effort[11] = 0.0;

      base_state.name[0] = " x ";
      base_state.position[0] = x_time;
      base_state.velocity[0] = 0.0;
      base_state.effort[0] = 0.0;

      base_state.name[1] = " y ";
      base_state.position[1] = y_time;
      base_state.velocity[1] = 0.0;
      base_state.effort[1] = 0.0;

      base_state.name[2] = " z ";
      base_state.position[2] = z_time;
      base_state.velocity[2] = 0.0;
      base_state.effort[2] = 0.0;

      base_state.name[3] = " roll ";
      base_state.position[3] = roll_time;
      base_state.velocity[3] = 0.0;
      base_state.effort[3] = 0.0;

      base_state.name[4] = " pitch ";
      base_state.position[4] = pitch_time;
      base_state.velocity[4] = 0.0;
      base_state.effort[4] = 0.0;

      base_state.name[5] = " yaw ";
      base_state.position[5] = yaw_time;
      base_state.velocity[5] = 0.0;
      base_state.effort[5] = 0.0;

      base_pub.publish(base_state);
      joint_pub.publish(joint_state);

      c_switch_msg.data = c_switch;
      c_switch_pub.publish(c_switch_msg);

      fine_msg.data = fine;
      fine_pub.publish(fine_msg);

      end_msg.data = endd;
      end_pub.publish(end_msg);

      scivolo_msg.data = scivolo;
      scivolo_pub.publish(scivolo_msg);

       std_msgs::Float64 conta_friction_msg;
      conta_friction_msg.data = conta_friction;
      conta_friction_pub.publish(conta_friction_msg);

      std_msgs::Float64 delta_x_avanti_msg;
      delta_x_avanti_msg.data = delta_x_avanti;
      delta_x_avanti_pub.publish(delta_x_avanti_msg);

      //cout<<"contatore"<<endl<< contatore<<endl;
      cout<<"time"<<endl<< t <<endl;
      cout<<"fine"<< endl << fine <<endl;
      cout<<"endd"<<endl<<endd<<endl;
      cout<<"c_switch"<<endl<<c_switch<<endl;
      cout<<"friction_cone"<<endl<<friction_cone<<endl;
      cout<<"friction_cone_dietro"<<endl<<friction_cone_dietro<<endl;
      cout<<"friction_dietro"<<endl<<friction_dietro<<endl;
      cout<<"friction_dietro_due"<<endl<<friction_dietro_due<<endl;
      cout<<"scivolo"<<endl<<scivolo<<endl;
      cout<<"scivolo_due"<<endl<<scivolo_due<<endl;
      cout<<"scivolo_dietro"<<endl<<scivolo_dietro<<endl;
      cout<<"scivolo_dietro_due"<<endl<<scivolo_dietro_due<<endl;
      cout<<"freeze"<<endl<<freeze<<endl;

      ros::spinOnce();
      loop_rate.sleep();

    }

  return 0;
}


/*******************************************************************************
 *         Copyright 2003-2012 LASMEA UMR 6602 CNRS/U.B.P
 *         Copyright 2011-2012 LRI    UMR 8623 CNRS/Univ Paris Sud XI
 *
 *          Distributed under the Boost Software License, Version 1.0.
 *                 See accompanying file LICENSE.txt or copy at
 *                     http://www.boost.org/LICENSE_1_0.txt
 ******************************************************************************/
#ifndef NT2_TOOLBOX_INTERPOL_FUNCTIONS_GENERIC_PCHIP_HPP_INCLUDED
#define NT2_TOOLBOX_INTERPOL_FUNCTIONS_GENERIC_PCHIP_HPP_INCLUDED

#include <nt2/toolbox/interpol/functions/pchip.hpp>
#include <nt2/include/functions/ppval.hpp>
#include <nt2/include/functions/is_nge.hpp>
#include <nt2/include/functions/is_nle.hpp>
#include <nt2/include/functions/issorted.hpp>
#include <nt2/include/functions/bsearch.hpp>
#include <nt2/include/functions/diff.hpp>
#include <nt2/include/functions/conj.hpp>
#include <nt2/include/functions/abs.hpp>
#include <nt2/include/functions/globalfind.hpp>
#include <nt2/include/functions/if_else.hpp>
#include <nt2/include/functions/is_gtz.hpp>
#include <nt2/include/functions/fma.hpp>
#include <nt2/include/functions/logical_or.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/oneminus.hpp>
#include <nt2/include/functions/oneplus.hpp>
#include <nt2/include/functions/repnum.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/include/functions/average.hpp>
#include <nt2/include/functions/first_index.hpp>
#include <nt2/include/functions/isreal.hpp>
#include <nt2/include/functions/colvect.hpp>
#include <nt2/include/functions/rowvect.hpp>
#include <nt2/include/functions/transpose.hpp>
#include <nt2/include/functions/reshape.hpp>
#include <nt2/include/functions/vertcat.hpp>
#include <nt2/include/functions/multiplies.hpp>
#include <nt2/include/functions/logical_and.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/three.hpp>
#include <nt2/core/container/table/table.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/simd/logical.hpp>
#include <boost/assert.hpp>

namespace nt2 { namespace ext
{

  NT2_FUNCTOR_IMPLEMENTATION( nt2::tag::run_assign_, tag::cpu_
                            , (A0)(A1)(N1)
                            , ((ast_<A0, nt2::container::domain>))
                              ((node_<A1,nt2::tag::pchip_,N1,nt2::container::domain>))
                            )
  {
    typedef typename boost::proto::result_of::child_c<A1&,0>::value_type  child0;
    typedef typename boost::proto::result_of::child_c<A1&,1>::value_type  child1;
    typedef typename boost::proto::result_of::child_c<A1&,2>::value_type  child2;
    typedef typename child0::value_type                               value_type;
    typedef typename meta::as_integer<value_type>::type               index_type;
    typedef table<value_type>                                             vtab_t;
    typedef table<index_type>                                             itab_t;
    typedef A0&                                                      result_type;

    result_type operator()(A0& yi, A1& inputs) const
    {
      yi.resize(inputs.extent());
      const child0 & x   =  boost::proto::child_c<0>(inputs);
      BOOST_ASSERT_MSG(issorted(x, 'a'), "for 'pchip' interpolation x values must be sorted in ascending order");
      const child1 & y   =  boost::proto::child_c<1>(inputs);
      const child2 & xi  =  boost::proto::child_c<2>(inputs);
      bool extrap = false;
      value_type extrapval = Nan<value_type>();
      choices(inputs, extrap, extrapval, N1());
      vtab_t h  =  nt2::diff(x,1,2);
      vtab_t del = nt2::diff(y,1,2)/h;
      pchipslopes(x,y,del, yi);
      ppval <value_type> pp(x,y,yi,h,del);
      yi =pp.eval(xi);
      if (!extrap)
      {
        value_type  b =  value_type(x(begin_));
        value_type  e =  value_type(x(end_));
        yi = nt2::if_else(nt2::logical_or(boost::simd::is_nge(xi, b),
                                          boost::simd::is_nle(xi, e)), extrapval, yi);
      }
      return yi;
    }
  private :
    static void choices(const A1&, bool &,  value_type&, boost::mpl::long_<3> const &)
    { }
    static void choices(const A1& inputs, bool & extrap,  value_type& extrapval, boost::mpl::long_<4> const &)
    {
      typedef typename boost::proto::result_of::child_c<A1&,3>::type             child3;
      typedef typename meta::scalar_of<child3>::type                    cref_param_type;
      typedef typename meta::strip<cref_param_type>::type                    param_type;
      get(inputs, extrap, extrapval, param_type());
    }
    static void get(const A1& inputs, bool & extrap,  value_type&,  const bool &)
    {
      extrap =  boost::proto::child_c<3>(inputs);
    }
    static void get(const A1& inputs, bool &,  value_type& extrapval,  const value_type &)
    {
      extrapval =  boost::proto::child_c<3>(inputs);
    }

    static void pchipslopes(const child0 & x, const child1 & y, const vtab_t &del, A0& d)
    {
      itab_t k;
      size_t n =  length(x);
      if (nt2::numel(x) == 2) {
        d =  nt2::repnum(value_type(del(begin_)), 1, width(y)); // del(begin_) is not of value_type !
      } else {
        d =  nt2::zeros(1, width(y), nt2::meta::as_<value_type>());
        if (/* nt2::isreal(del)*/ true) //to do proper version for real types
        { // is k 1 based or 0,  I hope 1 here ?
          k = nt2::globalfind(nt2::is_gtz(nt2::multiplies(nt2::sign(del(nt2::_(begin_, begin_+n-3))), nt2::sign(del(nt2::_(begin_+1, begin_+n-2))))), nt2::meta::as_<index_type>());
        }
        else
        {
          k = nt2::globalfind(nt2::logical_and(is_eqz(del(nt2::_(begin_, begin_+n-3))), is_eqz(del(nt2::_(begin_+1,begin_+n-2)))), nt2::meta::as_<index_type>());
        }
      }
      itab_t kp1 = oneplus(k);
      itab_t kp2 = oneplus(kp1);
      vtab_t h = nt2::diff(x, 1, 2);
      vtab_t hs = h(1, k)+h(1, kp1);
      vtab_t w1 = (h(1, k)+hs)/(Three<value_type>()*hs);
      vtab_t w2 = (hs+h(1, kp1))/(Three<value_type>()*hs);
      vtab_t dmax = nt2::max(nt2::abs(del(1, k)), nt2::abs(del(1, kp1)));
      vtab_t dmin = nt2::min(nt2::abs(del(1, k)), nt2::abs(del(1, kp1)));
      d(kp1) = dmin/nt2::conj(nt2::multiplies(w1,(del(1, k)/dmax)) + nt2::multiplies(w2, (del(1, kp1)/dmax)));
      //   Slopes at end points.
      //   Set d(0) and d(n-1) via non-centered, shape-preserving three-point formulae.
      d(1) = ((2*h(1)+h(2))*del(1) - h(1)*del(2))/(h(1)+h(2));
      if (/*nt2::isreal(d) && */(nt2::sign(d(nt2::first_index<1>(d))) != nt2::sign(del(1))))
      {
        d(nt2::first_index<2>(d)) = Zero<value_type>();
      }
      else if ((nt2::sign(del(1)) != nt2::sign(del(1))) &&
               (nt2::abs(d(nt2::first_index<1>(d))) > nt2::abs(Three<value_type>()*del(1))))
      {
        d(nt2::first_index<2>(d)) = Three<value_type>()*del(1);
      }
      //      index_type end = n;
      //     NT2_DISPLAY(h);
      d(nt2::last_index<2>(d)) = ((Two<value_type>()*h(n-1)+h(n-2))*del(n-1) - h(n-1)*del(n-2))/(h(n-1)+h(n-2));
      if (/*isreal(d) &&*/ (nt2::sign(d(nt2::last_index<1>(d))) != nt2::sign(del(n-1))))
      {
        d(nt2::last_index<2>(d)) = Zero<value_type>();
      }
      else if ((nt2::sign(del(n-1)) != nt2::sign(del(n-2))) &&
               (nt2::abs(d(nt2::last_index<1>(d))) > nt2::abs(Three<value_type>()*del(n-1))))
      {
        d(nt2::last_index<2>(d)) = 3*del(n-1);
      }
    }
  };
} }

#endif


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#pragma once

#include "RefWorkloadUtils.hpp"
#include "TensorBufferArrayView.hpp"

#include <armnn/Tensor.hpp>

#include <backendsCommon/DataLayoutIndexed.hpp>

#include <boost/assert.hpp>
#include <boost/numeric/conversion/cast.hpp>

#include <cmath>
#include <limits>

namespace armnn
{

/// Performs multiplication of an integer with a multiplier which is less than one,
/// using quantized integer arithmetic which is consistent with AndroidNN's CPU executor.
struct QuantizedMultiplierSmallerThanOne
{
public:
    /// Constructs a QuantizedMultiplierSmallerThanOne which will multiply by the given multiplier.
    /// This stores the appropriate integer quantities (derived from the given multiplier) for later use.
    /// The implementation of this function is adapted from Android NN's QuantizeMultiplierSmallerThanOne().
    QuantizedMultiplierSmallerThanOne(float multiplier);

    /// The implementation of this function is adapted from Android NN's MultiplyByQuantizedMultiplierSmallerThanOne().
    int32_t operator*(int32_t rhs) const;

private:
    /// The implementation of this function is adapted from gemmlowp's SaturatingRoundingDoublingHighMul().
    static int32_t SaturatingRoundingDoublingHighMul(int32_t a, int32_t b);

    /// The implementation of this function is adapted from gemmlowp's RoundingDivideByPOT().
    static int32_t RoundingDivideByPOT(int32_t x, int exponent);

    int32_t m_Multiplier;
    int32_t m_RightShift;
};

/// An implementation shared by normal and depthwise convolution.
template<typename ConvData, typename InputType, typename BiasType, typename AccumulatorType>
static void ConvImpl(ConvData data,
                     const InputType* inputData,
                     float inputScale,
                     int32_t inputOffset,
                     const InputType* filterData,
                     float filterScale,
                     int32_t filterOffset,
                     const BiasType* biasData,
                     InputType* outputData,
                     float outputScale,
                     int32_t outputOffset,
                     const TensorInfo& filterInfo,
                     bool depthwise = false)
{
    if (data.m_Parameters.m_BiasEnabled && !biasData)
    {
        throw InvalidArgumentException("Bias is enabled but the bias data is invalid");
    }

    const TensorInfo& inputInfo0  = GetTensorInfo(data.m_Inputs[0]);
    const TensorInfo& outputInfo0 = GetTensorInfo(data.m_Outputs[0]);

    TensorBufferArrayView<InputType> output(outputInfo0.GetShape(),
                                            GetOutputTensorData<InputType>(0, data),
                                            data.m_Parameters.m_DataLayout);

    const DataLayoutIndexed dataLayoutIndexed(data.m_Parameters.m_DataLayout);
    const unsigned int channelsIndex = dataLayoutIndexed.GetChannelsIndex();
    const unsigned int heightIndex   = dataLayoutIndexed.GetHeightIndex();
    const unsigned int widthIndex    = dataLayoutIndexed.GetWidthIndex();

    unsigned int depthMult      = depthwise ? filterInfo.GetShape()[0] : 1;
    unsigned int channelsInput  = filterInfo.GetShape()[channelsIndex];
    unsigned int channelsOutput = depthwise ? channelsInput * depthMult : filterInfo.GetShape()[0];

    unsigned int batchSize    = outputInfo0.GetShape()[0];
    unsigned int heightOutput = outputInfo0.GetShape()[heightIndex];
    unsigned int widthOutput  = outputInfo0.GetShape()[widthIndex];
    unsigned int heightInput  = inputInfo0.GetShape()[heightIndex];
    unsigned int widthInput   = inputInfo0.GetShape()[widthIndex];

    unsigned int heightFilter = filterInfo.GetShape()[heightIndex];
    unsigned int widthFilter  = filterInfo.GetShape()[widthIndex];

    unsigned int paddingTop = data.m_Parameters.m_PadTop;
    unsigned int paddingLeft = data.m_Parameters.m_PadLeft;
    unsigned int hStride  = data.m_Parameters.m_StrideY;
    unsigned int xStride  = data.m_Parameters.m_StrideX;

    // The world's least efficient convolution.
    for (unsigned int batchIdx = 0; batchIdx < batchSize; batchIdx++)
    {
        for (unsigned int cOutput = 0; cOutput < channelsOutput; cOutput++)
        {
            for (unsigned int yOutput = 0; yOutput < heightOutput; yOutput++)
            {
                for (unsigned int xOutput = 0; xOutput < widthOutput; xOutput++)
                {
                    // This loop goes over each output element.
                    AccumulatorType sum = AccumulatorType();

                    // For depthwise, each output channel corresponds to exactly one input channel.
                    // For normal, must loop over each input channel.
                    for (unsigned int cInput = 0; cInput < (depthwise ? 1 : channelsInput); cInput++)
                    {
                        unsigned int depthwiseMultiplierIdx = 0;
                        if (depthwise)
                        {
                            cInput = cOutput / depthMult;
                            depthwiseMultiplierIdx = cOutput % depthMult;
                        }

                        for (unsigned int yFilter = 0; yFilter < heightFilter; yFilter++)
                        {
                            for (unsigned int xFilter = 0; xFilter < widthFilter; xFilter++)
                            {
                                // This loop goes over each input element for each output element.

                                unsigned int filterIndex;

                                // Since dimensionality of kernel depends on depthwiseness, so does index.
                                if (depthwise)
                                {
                                    if (data.m_Parameters.m_DataLayout == DataLayout::NHWC)
                                    {
                                        filterIndex = depthwiseMultiplierIdx * heightFilter * widthFilter
                                                        * channelsInput +
                                                      yFilter * widthFilter * channelsInput +
                                                      xFilter * channelsInput +
                                                      cInput;
                                    }
                                    else
                                    {
                                        filterIndex = depthwiseMultiplierIdx * widthFilter * heightFilter
                                                        * channelsInput +
                                                      cInput * widthFilter * heightFilter +
                                                      yFilter * widthFilter +
                                                      xFilter;
                                    }
                                }
                                else
                                {
                                    if (data.m_Parameters.m_DataLayout == DataLayout::NHWC)
                                    {
                                        filterIndex = cOutput * heightFilter * widthFilter * channelsInput +
                                                      yFilter * widthFilter * channelsInput +
                                                      xFilter * channelsInput +
                                                      cInput;
                                    }
                                    else
                                    {
                                        filterIndex = cOutput * widthFilter * heightFilter * channelsInput +
                                                      cInput  * widthFilter * heightFilter +
                                                      yFilter * widthFilter +
                                                      xFilter;
                                    }
                                }

                                AccumulatorType filterValue = filterData[filterIndex] -
                                    boost::numeric_cast<AccumulatorType>(filterOffset);

                                unsigned int yInput = yOutput * hStride + yFilter;
                                unsigned int xInput = xOutput * xStride + xFilter;

                                AccumulatorType inputValue;

                                // Check if we're in the padding.
                                if (yInput < paddingTop || yInput >= heightInput + paddingTop ||
                                    xInput < paddingLeft || xInput >= widthInput + paddingLeft )
                                {
                                    inputValue = AccumulatorType();
                                }
                                else
                                {
                                    unsigned int inputIndex;

                                    if (data.m_Parameters.m_DataLayout == DataLayout::NHWC)
                                    {
                                        inputIndex = batchIdx * heightInput * widthInput  * channelsInput +
                                                     (yInput - paddingTop) * widthInput * channelsInput +
                                                     (xInput - paddingLeft) * channelsInput +
                                                     cInput;

                                    }
                                    else
                                    {
                                        inputIndex = batchIdx * widthInput * heightInput * channelsInput +
                                                     widthInput * heightInput * cInput +
                                                     widthInput * (yInput - paddingTop) +
                                                     xInput - paddingLeft;
                                    }

                                    inputValue = inputData[inputIndex] -
                                                    boost::numeric_cast<AccumulatorType>(inputOffset);

                                }
                                sum += filterValue * inputValue;
                            }
                        }
                    }

                    if (data.m_Parameters.m_BiasEnabled)
                    {
                        sum += biasData[cOutput];
                    }

                    if (outputScale != 0.0f)
                    {
                        float multiplier = (inputScale * filterScale) / outputScale;
                        // Apply the multiplier to sum, but do so using some quantized arithmetic which is consistent
                        // with the AndroidNN CPU implementation. This should be (roughly) equivalent to:
                        //  sum = std::round(multiplier * sum + outputOffset);
                        sum = boost::numeric_cast<AccumulatorType>(
                                QuantizedMultiplierSmallerThanOne(multiplier) * boost::numeric_cast<int32_t>(sum))
                            + boost::numeric_cast<AccumulatorType>(outputOffset);
                        sum = std::min<AccumulatorType>(std::max<AccumulatorType>(sum, 0), 255);
                    }

                    output.Get(batchIdx, cOutput, yOutput, xOutput) = boost::numeric_cast<InputType>(sum);
                }
            }
        }
    }
}

} //namespace armnn


#include <opencv2/opencv.hpp>
#include <string>
#include <iostream>
#include <Eigen/Eigen>
#include <vector>
#include <chrono>
#include <Eigen/Core>
#include <Eigen/Dense>
#include <opencv2/core/eigen.hpp>
#include "bvftdescriptors.h"
#include <queue>
#include <fstream>
#include <string>
#include <math.h>
#include <stdlib.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <python2.7/Python.h>

using namespace std;
using namespace cv;
using namespace Eigen;

int readPointCloud(pcl::PointCloud<pcl::PointXYZ> &point_cloud, const std::string filename)
{
  point_cloud.clear();
  std::ifstream binfile(filename.c_str(),std::ios::binary);
  if(!binfile)
  {
    throw std::runtime_error("file \"" +filename+ "\" cannot open");
    return -1;
  }
  else
  {
    std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
    vector<float> tmp;
    while(1)
    {
      double s;
      pcl::PointXYZ point;
      binfile.read((char*)&s,sizeof(double));
      if(binfile.eof()) break;
      tmp.push_back(s);
      point.x = s;
      binfile.read((char*)&s,sizeof(double));
      tmp.push_back(s);
      point.y = s;
      binfile.read((char*)&s,sizeof(double));
      tmp.push_back(s);
      point.z = s;
      point_cloud.push_back(point);
   }

   std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
  }
  binfile.close();
  return 1;
}

std::vector<std::string> pyListSortDir(std::string path)
{
    std::vector<std::string> ret;

    Py_Initialize();

    //PyRun_SimpleString("import os");
    PyObject* module_name = PyString_FromString("os");
    PyObject* os_module = PyImport_Import(module_name);
    PyObject* os_list = PyObject_GetAttrString(os_module, "listdir");
    //PyObject* os_list = PyObject_GetAttrString(os_module, "");

    PyObject *ArgList = PyTuple_New(1);
    PyObject* py_path = PyString_FromString(path.c_str());
    PyTuple_SetItem(ArgList, 0, py_path);

    PyObject* files = PyObject_CallObject(os_list, ArgList);
    //PyObject*
    PyList_Sort(files);
    //PyObject_CallMethod(PyList,"sort", 'O', files);
    //PyObject* files_sort_ = PyObject_CallObject(files_sort,ArgList);
    for(int i=0; i<PyList_Size(files); i++)
    {
        char  *temp;
        PyObject* item = PyList_GetItem(files,i);
        //std::cout << item << std::endl;
        PyArg_Parse(item,"s",&temp);
        //std::cout << std::string(temp) << std::endl;
        ret.push_back(std::string(temp));
    }
    //PyObject* re = PyRun_SimpleString("os.list()");
    Py_Finalize();
    return ret;

}

void generateBVImage(pcl::PointCloud<pcl::PointXYZ> &point_cloud, int &x_max_ind, int &y_max_ind, Mat &mat_local_image)
{
    float resolution = 0.4;
    pcl::VoxelGrid<pcl::PointXYZ> down_size_filter;
    down_size_filter.setLeafSize(resolution, resolution, resolution/2);
    down_size_filter.setInputCloud(point_cloud.makeShared());
    down_size_filter.filter(point_cloud);

    float x_min = 10000, y_min = 10000, x_max=-100000,y_max=-100000;
    for(int i=0; i<point_cloud.size(); i++)
    {
        if(point_cloud.points[i].y< x_min) x_min=point_cloud.points[i].y;
        if(point_cloud.points[i].y> x_max) x_max=point_cloud.points[i].y;
        if(point_cloud.points[i].x< y_min) y_min=point_cloud.points[i].x;
        if(point_cloud.points[i].x> y_max) y_max=point_cloud.points[i].x;
    }
    int x_min_ind = int(x_min/resolution);
    x_max_ind = int(x_max/resolution);
    int y_min_ind = int(y_min/resolution);
    y_max_ind = int(y_max/resolution);

    int x_num = x_max_ind-x_min_ind+1;
    int y_num = y_max_ind-y_min_ind+1;
    mat_local_image=Mat( y_num, x_num,CV_8UC1, cv::Scalar::all(0));

    for(int i=0; i<point_cloud.size(); i++)
    {
        int x_ind = x_max_ind-int((point_cloud.points[i].y)/resolution);
        int y_ind = y_max_ind-int((point_cloud.points[i].x)/resolution);
        if(x_ind>=x_num || y_ind>=y_num ) continue;
        mat_local_image.at<uint8_t>( y_ind,x_ind) += 1;
    }
    uint8_t max_pixel = 0;
    for(int i=0; i<x_num; i++)
        for(int j=0; j<y_num; j++)
        {
            if (mat_local_image.at<uint8_t>(j, i)>max_pixel) max_pixel=mat_local_image.at<uint8_t>(j, i);
        }
    for(int i=0; i<x_num; i++)
        for(int j=0; j<y_num; j++)
        {
            if(mat_local_image.at<uint8_t>(j, i)*10>100) {mat_local_image.at<uint8_t>(j, i)=100;continue;}
            mat_local_image.at<uint8_t>(j, i)=uint8_t(mat_local_image.at<uint8_t>(j, i)*10);
            if(uint8_t(mat_local_image.at<uint8_t>(j, i))==0) {mat_local_image.at<uint8_t>(j, i)=10;continue;}
        }
}

Mat max_moment_local;
Mat MIM_global;
Mat MIM_local;

int max_global_x_ind;
int max_global_y_ind;

int imagePadding(Mat& img, int &cor_x, int & cor_y)
{
    pad_size=200;
    copyMakeBorder(img, img, pad_size/2,pad_size/2,pad_size/2,pad_size/2, BORDER_CONSTANT, Scalar(10));

	int m = getOptimalDFTSize(img.rows);
	int n = getOptimalDFTSize(img.cols);

    int row_pad = (m - img.rows)/2;
    int col_pad = (n - img.cols)/2;
    //take this step to make fft faster
    copyMakeBorder(img, img,row_pad , (m - img.rows)%2?row_pad+1:row_pad,
                col_pad, (n - img.cols)%2?col_pad+1:col_pad , BORDER_CONSTANT, Scalar(10));
    cor_x += col_pad+pad_size/2;
    cor_y += row_pad+pad_size/2;
}

void match2Image(Mat img1, Mat img2, int max_x1, int max_y1, int max_x2 , int max_y2)
{
    float rows = img1.rows, cols = img1.cols;
    BVFT bvfts1 = detectBVFT(img1);
    BVFT bvfts2 = detectBVFT(img2);

    bvfts2.keypoints.insert(bvfts2.keypoints.end(),bvfts2.keypoints.begin(),bvfts2.keypoints.end());

    Mat temp(bvfts2.keypoints.size(),bvfts2.descriptors.cols,CV_32F,Scalar{0} );
    bvfts2.descriptors.copyTo(temp(Rect(0,0,bvfts2.descriptors.cols,bvfts2.keypoints.size()/2)));
    int areas = 6;
    int feautre_size=bvfts2.descriptors.cols/areas/areas;
    for(int i=0; i<areas*areas; i++)  //areas*areas
    {
        bvfts2.descriptors(Rect((areas*areas-i-1)*feautre_size,0 , feautre_size,bvfts2.keypoints.size()/2)).copyTo(temp(Rect(i*feautre_size, bvfts2.keypoints.size()/2, feautre_size , bvfts2.keypoints.size()/2))); //i*norient
    }
    bvfts2.descriptors = temp.clone();

    BFMatcher matcher;//(NORM_L2, true);
	vector<DMatch> matches;
	matcher.match(bvfts1.descriptors, bvfts2.descriptors, matches);

    vector<Point2f> points1;
    vector<Point2f> points2;
    vector<DMatch>::iterator it_end = matches.end();
    for(vector<DMatch>::iterator it= matches.begin(); it!= it_end;it++)
    {
        Point2f point_local_1 = (Point2f(max_y1,max_x1)-
                                    Point2f(bvfts1.keypoints[it->queryIdx].pt.y,bvfts1.keypoints[it->queryIdx].pt.x))*0.4;
        points1.push_back(point_local_1);

        point_local_1 = (Point2f(max_y2,max_x2)-
                                    Point2f(bvfts2.keypoints[it->trainIdx].pt.y,bvfts2.keypoints[it->trainIdx].pt.x))*0.4;
        points2.push_back(point_local_1);
    }
    cv::Mat keypoints1(points1);
    keypoints1=keypoints1.reshape(1,keypoints1.rows); //N*2
    cv::Mat keypoints2(points2);
    keypoints2=keypoints2.reshape(1,keypoints2.rows);

    Mat inliers;

    vector<int> inliers_ind;
    Mat rigid = estimateICP(keypoints1, keypoints2, inliers_ind);

    if (inliers_ind.size()<4) cout << "few inlier points" << endl;
    cout << "find transform: \n" <<  rigid << endl;
    //return;

    vector<DMatch> good_matches;

    for(int i=0; i<inliers_ind.size(); i++)
      good_matches.push_back(matches[inliers_ind[i]]);

    Mat matchesGoodImage;

    for(int i=0; i<img1.rows; i++)
      for(int j=0; j<img1.cols; j++)
        if(img1.ptr<uint8_t>(i)[j]<=10) img1.ptr<uint8_t>(i)[j]=10;

    for(int i=0; i<img2.rows; i++)
      for(int j=0; j<img2.cols; j++)
        if(img2.ptr<uint8_t>(i)[j]<=10) img2.ptr<uint8_t>(i)[j]=10;

    normalize(img1, img1, 0, 255, CV_MINMAX);
    normalize(img2, img2, 0, 255, CV_MINMAX);

    drawKeypoints(img1, bvfts1.keypoints,img1,Scalar(0,0,255),DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
    drawKeypoints(img2, bvfts2.keypoints,img2,Scalar(0,0,255),DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

    drawMatches(img1, bvfts1.keypoints, img2, bvfts2.keypoints, good_matches, matchesGoodImage, Scalar::all(-1),
		Scalar::all(-1), vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);

    imshow("matchesGoodImage", matchesGoodImage);
    imwrite("match.png",matchesGoodImage);
    waitKey(0);
}

int main(int argc, char** argv)
{

    string img1_path = argv[1];
    string img2_path = argv[2];

    pcl::PointCloud<pcl::PointXYZ> point_cloud1;
    pcl::PointCloud<pcl::PointXYZ> point_cloud2;

    //read point clouds
    readPointCloud(point_cloud1, img1_path);
    readPointCloud(point_cloud2, img2_path);

    //apply rotation transform to test the rotation invariance
    float rotation_angle=137.0/180*CV_PI;
    for(int i=0; i<point_cloud1.size(); i++)
    {
        pcl::PointXYZ tmp = point_cloud1[i];
        point_cloud1[i].x = cos(rotation_angle)*tmp.x + sin(rotation_angle)*tmp.y;
        point_cloud1[i].y = -sin(rotation_angle)*tmp.x + cos(rotation_angle)*tmp.y;
    }

    Mat img1, img2;
    int max_x1,max_y2,max_x2,max_y1;  //used for localizing the center of the images

    //generate bv images
    generateBVImage(point_cloud1, max_x1, max_y1, img1);
    generateBVImage(point_cloud2, max_x2, max_y2, img2);

    //padding to make fft faster
    imagePadding(img1, max_x1, max_y1);
    imagePadding(img2, max_x2, max_y2);

    //perform matching
    match2Image(img1, img2, max_x1,max_y1,max_x2,max_y2);
}


/*
Copyright (c) 2014, Kai Klindworth
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
   list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
   this list of conditions and the following disclaimer in the documentation
   and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

#include "ml_region_optimizer.h"

#include <omp.h>
#include "disparity_region.h"
#include "disparity_region_algorithms.h"
#include <segmentation/intervals.h>
#include <segmentation/intervals_algorithms.h>
#include "disparity_toolkit/disparity_utils.h"
#include "disparity_toolkit/genericfunctions.h"
#include "disparity_toolkit/disparity_range.h"

#include <fstream>
#include <boost/lexical_cast.hpp>
#include <neural_network/network.h>
#include <neural_network/data_normalizer.h>
#include "costmap_utils.h"
#include "debugmatstore.h"
#include "ml_region_optimizer_algorithms.h"

using namespace neural_network;

void refresh_base_optimization_vector_internal(std::vector<std::vector<float>>& optimization_vectors, const region_container& base, const region_container& match, int delta)
{
	int pot_trunc = 15;

	std::vector<ml_feature_calculator> hyp_vec(omp_get_max_threads(), ml_feature_calculator(base, match));
	std::size_t regions_count = base.regions.size();

	optimization_vectors.resize(regions_count);
	#pragma omp parallel for
	for(std::size_t i = 0; i < regions_count; ++i)
	{
		const disparity_region& baseRegion = base.regions[i];
		int thread_idx = omp_get_thread_num();

		disparity_range drange = task_subrange(base.task, baseRegion.base_disparity, delta);
		hyp_vec[thread_idx](baseRegion, pot_trunc, drange, optimization_vectors[i]);
	}
}

void ml_region_optimizer_base::refresh_base_optimization_vector(const region_container& left, const region_container& right, int delta)
{
	refresh_base_optimization_vector_internal(feature_vectors_left, left, right, delta);
	refresh_base_optimization_vector_internal(feature_vectors_right, right, left, delta);
}

/**
 * @brief ml_region_optimizer::optimize_ml
 * @param base Region container
 * @param match Region container
 * @param optimization_vectors_base
 * @param optimization_vectors_match
 * @param delta
 * @param filename
 */
void ml_region_optimizer::optimize_ml(region_container& base, const region_container& match, std::vector<std::vector<float>>& optimization_vectors_base, std::vector<std::vector<float>>& optimization_vectors_match, int delta, const std::string& filename)
{
	std::cout << "optimize" << std::endl;

	std::ifstream istream(filename);

	if(!istream.is_open())
		throw std::runtime_error("file not found: " + filename);

	//load all normalization factors
	data_normalizer<double> normalizer(istream);
	//load weights
	istream >> *nnet;

	const std::size_t regions_count = base.regions.size();
	const short sign = (base.task.range.start() < 0) ? -1 : 1;

	std::vector<std::vector<double>> region_feature_vectors(omp_get_max_threads(), std::vector<double>(base.task.range.size()*vector_size_per_disp*2+vector_size));
	#pragma omp parallel for
	for(std::size_t j = 0; j < regions_count; ++j)
	{
		std::vector<double>& feature_vector = region_feature_vectors[omp_get_thread_num()];
		//merge feature vectors for a image region and corresponding (weighted) region, for all disparities.
		merge_with_corresponding_feature_vector<ml_region_optimizer::vector_size, ml_region_optimizer::vector_size_per_disp>(feature_vector.data(), base.regions[j], optimization_vectors_base[j], optimization_vectors_match, match, delta, base.task);

		normalizer.apply(feature_vector);
		base.regions[j].disparity = nnet->predict(feature_vector) * sign;
	}

	refresh_warped_regions(base);
}

void ml_region_optimizer::prepare_training_sample(std::vector<short>& dst_gt, std::vector<std::vector<double>>& dst_data, const std::vector<std::vector<float>>& base_feature_vectors, const std::vector<std::vector<float>>& match_feature_vectors, const region_container& base, const region_container& match, int delta)
{
	dst_gt.reserve(dst_gt.size() + base.regions.size());
	std::vector<short> gt;
	gt.reserve(base.regions.size());
	average_region_ground_truth(base.regions, base.task.groundTruth, std::back_inserter(gt));

	const int crange = base.task.range.size();

	const std::size_t regions_count = base.regions.size();
	dst_data.reserve(dst_data.size() + base_feature_vectors.size());

	assert(gt.size() == regions_count);
	//#pragma omp parallel for default(none) shared(base, match, delta, normalization_vector)
	result_eps_calculator diff_calc;
	for(std::size_t j = 0; j < regions_count; ++j)
	{
		if(gt[j] != 0)
		{
			dst_data.emplace_back(vector_size_per_disp*2*crange+vector_size);
			double *dst_ptr = dst_data.back().data();
			merge_with_corresponding_feature_vector<vector_size, vector_size_per_disp>(dst_ptr, base.regions[j], base_feature_vectors[j], match_feature_vectors, match, delta, base.task);

			dst_gt.push_back(gt[j]);

			//std::cout << base.regions[j].disparity << " vs " << gt[j] << std::endl;
			diff_calc(base.regions[j].disparity, gt[j]);
		}
	}
	std::cout << diff_calc << std::endl;
}

void ml_region_optimizer_base::run(region_container& left, region_container& right, const optimizer_settings& /*config*/, int refinement)
{
	refresh_base_optimization_vector(left, right, refinement);

	const int optimization_iterations = training_mode ? training_iteration - 1: training_iteration; //in training mode, we want to learn the last optimization iteration, therefore optimize one interation less

	for(int i = 0; i <= optimization_iterations; ++i)
	{
		optimize_ml(left, right, feature_vectors_left, feature_vectors_right, refinement, filename_left_prefix  + std::to_string(i) + ".txt");
		optimize_ml(right, left, feature_vectors_right, feature_vectors_left, refinement, filename_right_prefix + std::to_string(i) + ".txt");

		refresh_base_optimization_vector(left, right, refinement);
	}

	if(training_mode)
	{
		prepare_training_sample(samples_gt_left,  samples_left,  feature_vectors_left, feature_vectors_right, left, right, refinement);
		prepare_training_sample(samples_gt_right, samples_right, feature_vectors_right, feature_vectors_left, right, left, refinement);
	}
	else
	{
		std::vector<short> gt;
		average_region_ground_truth(left.regions, left.task.groundTruth, std::back_inserter(gt));

		result_eps_calculator diff_calc = get_region_comparision(left.regions, gt);
		matstore.add_mat(get_region_gt_error_image(left, gt), "gt_diff");

		total_diff_calc += diff_calc;

		std::cout << diff_calc << std::endl;
		std::cout << "total: " << total_diff_calc << std::endl;
	}
}

void init_network(network<double>& net, int crange, int nvector, int pass)
{
	/*//net.emplace_layer<vector_extension_layer>(ml_region_optimizer::vector_size_per_disp, ml_region_optimizer::vector_size);
	//net.emplace_layer<vector_connected_layer>(nvector*2, nvector*2, pass);
	//net.emplace_layer<relu_layer>();
	net.emplace_layer<vector_connected_layer>(nvector*2, nvector*2, pass);
	net.emplace_layer<relu_layer>();
	net.emplace_layer<transpose_vector_connected_layer>(4, nvector*2, pass);
	net.emplace_layer<relu_layer>();
	net.emplace_layer<row_connected_layer>(crange, crange, pass);
	net.emplace_layer<relu_layer>();
	//net.emplace_layer<fully_connected_layer>(crange);
	//net.emplace_layer<relu_layer>();
	net.emplace_layer<fully_connected_layer>(crange);
	net.emplace_layer<softmax_output_layer>();*/

	net.emplace_layer<vector_connected_layer>(nvector*2, nvector, pass);
	net.emplace_layer<relu_layer>();
	net.emplace_layer<vector_connected_layer>(nvector, nvector*2, pass);
	net.emplace_layer<relu_layer>();
	net.emplace_layer<fully_connected_layer>(nvector);
	net.emplace_layer<relu_layer>();
	net.emplace_layer<fully_connected_layer>(nvector);
	net.emplace_layer<relu_layer>();
	//net.emplace_layer<fully_connected_layer>(4);
	//net.emplace_layer<relu_layer>();
	net.emplace_layer<fully_connected_layer>(crange);
	net.emplace_layer<softmax_output_layer>();

}

void ml_region_optimizer::reset_internal()
{
	samples_left.clear();
	samples_right.clear();

	range = 256;
	int dims = range * vector_size_per_disp*2+vector_size;
	//int nvector = ml_region_optimizer::vector_size_per_disp + ml_region_optimizer::vector_size;
	int nvector = ml_region_optimizer::vector_size_per_disp;
	int pass = ml_region_optimizer::vector_size;
	nnet = std::make_unique<network<double>>(dims);

	init_network(*nnet, range, nvector, pass);
}

ml_region_optimizer::ml_region_optimizer()
{
	reset_internal();
	training_iteration = 0;
	filename_left_prefix = "weights-left-";
	filename_right_prefix = "weights-right-";

	_settings.batch_size = 64;
	_settings.epochs = 61;
	_settings.training_error_calculation = 4;
}

ml_region_optimizer::~ml_region_optimizer()
{
}

void ml_region_optimizer::reset(const region_container& /*left*/, const region_container& /*right*/)
{
	reset_internal();
}

void training_internal(std::vector<std::vector<double>>& samples, std::vector<short>& samples_gt, const std::string& filename, neural_network::training_settings settings, int crange)
{
	std::cout << "start actual training" << std::endl;

	data_normalizer<double> normalizer(ml_region_optimizer::vector_size_per_disp, ml_region_optimizer::vector_size);
	normalizer.gather(samples);
	normalizer.apply(samples);

	randomize_dataset(samples, samples_gt);

	//class statistics
	/*std::vector<unsigned int> stats(crange, 0);
	for(std::size_t i = 0; i < samples_gt.size(); ++i)
		++(stats[std::abs(samples_gt[i])]);
	for(std::size_t i = 0; i < stats.size(); ++i)
		std::cout << "[" << i << "] " << (float)stats[i]/samples_gt.size() << ", " << (float)stats[i]/samples_gt.size()/(1.0/crange) << "\n";
	std::cout << std::endl;*/

	int dims = samples.front().size();
	assert(dims == (ml_region_optimizer::vector_size_per_disp*2*crange)+ml_region_optimizer::vector_size);
	network<double> net(dims);

	//int nvector = ml_region_optimizer::vector_size_per_disp + ml_region_optimizer::vector_size;
	int nvector = ml_region_optimizer::vector_size_per_disp;
	int pass = ml_region_optimizer::vector_size;

	init_network(net, crange, nvector, pass);
	net.training(samples, samples_gt, settings);

	std::ofstream ostream(filename);
	ostream.precision(17);
	normalizer.write(ostream);

	ostream << net;
	ostream.close();

	std::cout << "fin" << std::endl;
}

void ml_region_optimizer::training()
{
	training_internal(samples_left, samples_gt_left, filename_left_prefix + std::to_string(training_iteration) + ".txt", _settings, range);
	training_internal(samples_right, samples_gt_right, filename_right_prefix + std::to_string(training_iteration) + ".txt", _settings, range);
}

void ml_feature_calculator::update_result_vector(std::vector<float>& result_vector, const disparity_region& baseRegion, const disparity_range& drange)
{
	const int range = drange.size();
	const int dispMin = drange.offset();

	const neighbor_values neigh = get_neighbor_values(baseRegion, drange);

	//	float costs, occ_avg, neighbor_pot, lr_pot ,neighbor_color_pot;
	std::size_t feature_vector_size = range*ml_region_optimizer::vector_size_per_disp+ml_region_optimizer::vector_size;
	result_vector.resize(feature_vector_size);
	const float org_size = baseRegion.size();
	float *result_ptr = result_vector.data();
	for(int i = 0; i < range; ++i)
	{
		*result_ptr++ = cost_values[i];
		*result_ptr++ = occ_avg_values[i];
		*result_ptr++ = neighbor_pot_values[i];
		*result_ptr++ = lr_pot_values[i];
		*result_ptr++ = neighbor_color_pot_values[i];
		*result_ptr++ = (float)occ_temp[i].first / org_size;
		//*result_ptr++ = rel_cost_values[i];
		int hyp_disp = std::abs(dispMin + i);
		*result_ptr++ = neigh.left.disparity - hyp_disp;
		*result_ptr++ = neigh.right.disparity - hyp_disp;
		*result_ptr++ = neigh.top.disparity - hyp_disp;
		*result_ptr++ = neigh.bottom.disparity - hyp_disp;
		*result_ptr++ = warp_costs_values[i];
	//}
	//*result_ptr = baseRegion.disparity;
	*result_ptr++ = *std::min_element(cost_values.begin(), cost_values.end());
	*result_ptr++ = neigh.left.color_dev;
	*result_ptr++ = neigh.right.color_dev;
	*result_ptr++ = neigh.top.color_dev;
	*result_ptr++ = neigh.bottom.color_dev;
	*result_ptr++ = *std::min_element(warp_costs_values.begin(), warp_costs_values.end());
	*result_ptr++ = *std::min_element(lr_pot_values.begin(), lr_pot_values.end());
	*result_ptr++ = *std::min_element(neighbor_color_pot_values.begin(), neighbor_color_pot_values.end());

	*result_ptr++ = stats.mean;
	*result_ptr++ = stats.stddev;
}

	assert((result_ptr - result_vector.data()) == feature_vector_size);
}

float create_min_version(std::vector<float>::iterator start, std::vector<float>::iterator end, std::vector<float>::iterator ins)
{
	float min_value = *(std::min_element(start, end));

	std::transform(start, end, ins, [min_value](float val){
		return val - min_value;
	});

	return min_value;
}

void ml_feature_calculator::operator()(const disparity_region& baseRegion, short pot_trunc, const disparity_range& drange, std::vector<float>& result_vector)
{
	const int range = drange.size();

	cost_values.resize(range);
	rel_cost_values.resize(range);

	update_warp_costs(baseRegion, drange);
	update_occ_avg(baseRegion, pot_trunc, drange);
	update_average_neighbor_values(baseRegion, pot_trunc, drange);
	update_lr_pot(baseRegion, pot_trunc, drange);
	generate_stats(baseRegion, stats);

	for(int i = 0; i < range; ++i)
		cost_values[i] = baseRegion.disparity_costs((drange.start()+i)-baseRegion.disparity_offset);

	create_min_version(cost_values.begin(), cost_values.end(), rel_cost_values.begin());

	update_result_vector(result_vector, baseRegion, drange);
}


/**  \file posterior_mcmc.hpp \brief Posterior distribution on GPs
     based on MCMC over kernel parameters */
/*
-------------------------------------------------------------------------
   This file is part of BayesOpt, an efficient C++ library for
   Bayesian optimization.

   Copyright (C) 2011-2015 Ruben Martinez-Cantin <rmcantin@unizar.es>

   BayesOpt is free software: you can redistribute it and/or modify it
   under the terms of the GNU Affero General Public License as published by
   the Free Software Foundation, either version 3 of the License, or
   (at your option) any later version.

   BayesOpt is distributed in the hope that it will be useful, but
   WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU Affero General Public License for more details.

   You should have received a copy of the GNU Affero General Public License
   along with BayesOpt.  If not, see <http://www.gnu.org/licenses/>.
------------------------------------------------------------------------
*/

#ifndef  _POSTERIOR_MCMC_HPP_
#define  _POSTERIOR_MCMC_HPP_

#include <boost/ptr_container/ptr_vector.hpp>
#include "criteria_functors.hpp"
#include "posteriormodel.hpp"
#include "mcmc_sampler.hpp"

namespace bayesopt {

  /**
   * \brief Posterior model of nonparametric processes/criteria based
   * on MCMC samples.
   *
   * For computational reasons we store a copy of each conditional
   * models with the corresponding particle generated by MCMC. That is
   * to avoid costly operations like matrix inversions for every
   * kernel parameter in a GP prediction. Thus, we assume that the
   * number of particles is not very large.
   */
  class MCMCModel: public PosteriorModel
  {
  public:

    typedef boost::ptr_vector<NonParametricProcess>  GPVect;
    typedef boost::ptr_vector<Criteria>  CritVect;

    /**
     * \brief Constructor (Note: default constructor is private)
     *
     * @param dim number of input dimensions
     * @param params configuration parameters (see parameters.hpp)
     * @param eng random number generation engine (boost)
     */
    MCMCModel(size_t dim, Parameters params, randEngine& eng);

    virtual ~MCMCModel();

    void updateHyperParameters();
    void fitSurrogateModel();
    void updateSurrogateModel();

    double evaluateCriteria(const vectord& query);
    void updateCriteria(const vectord& query);

    bool criteriaRequiresComparison();
    void setFirstCriterium();
    bool setNextCriterium(const vectord& prevResult);
    std::string getBestCriteria(vectord& best);

    ProbabilityDistribution* getPrediction(const vectord& query);

  private:
    void setSurrogateModel(randEngine& eng);
    void setCriteria(randEngine& eng);

  private:  // Members
    size_t nParticles;
    GPVect mGP;                ///< Pointer to surrogate model
    CritVect mCrit;                    ///< Metacriteria model

    boost::scoped_ptr<MCMCSampler> kSampler;

  private: //Forbidden
    MCMCModel();
    MCMCModel(MCMCModel& copy);
  };

  /**@}*/

  inline void MCMCModel::fitSurrogateModel()
  {
    for(GPVect::iterator it=mGP.begin(); it != mGP.end(); ++it)
      it->fitSurrogateModel();
  };

  inline void MCMCModel::updateSurrogateModel()
  {
    for(GPVect::iterator it=mGP.begin(); it != mGP.end(); ++it)
      it->updateSurrogateModel();
  };

  inline double MCMCModel::evaluateCriteria(const vectord& query)
  {
    double sum = 0.0;
    for(CritVect::iterator it=mCrit.begin(); it != mCrit.end(); ++it)
      {
	sum += it->evaluate(query);
      }
    return sum/static_cast<double>(nParticles);
  };

  inline void MCMCModel::updateCriteria(const vectord& query)
  {
    for(CritVect::iterator it=mCrit.begin(); it != mCrit.end(); ++it)
      {
	it->update(query);
      }
  };

  inline bool MCMCModel::criteriaRequiresComparison()
  {return mCrit[0].requireComparison(); };

  inline void MCMCModel::setFirstCriterium()
  {
    for(CritVect::iterator it=mCrit.begin(); it != mCrit.end(); ++it)
      {
	it->initialCriteria();
      }
  };

  // Although we change the criteria for all MCMC particles, we use
  // only the first element to compute de Hedge algorithm, because it
  // should be based on the average result, thus being common for all
  // the particles.
  inline bool MCMCModel::setNextCriterium(const vectord& prevResult)
  {
    bool rotated;
    mCrit[0].pushResult(prevResult);
    for(CritVect::iterator it=mCrit.begin(); it != mCrit.end(); ++it)
      {
	rotated = it->rotateCriteria();
      }
    return rotated;
  };

  inline std::string MCMCModel::getBestCriteria(vectord& best)
  { return mCrit[0].getBestCriteria(best); };

  inline
  ProbabilityDistribution* MCMCModel::getPrediction(const vectord& query)
  { return mGP[0].prediction(query); };

} //namespace bayesopt

#endif


#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <boost/foreach.hpp>

#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;

// not good practice
ros::Publisher pub;

void callback(const PointCloud::ConstPtr& cloud)
{
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  // Create the segmentation object
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  // Optional
  seg.setOptimizeCoefficients (true);
  // Mandatory
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setDistanceThreshold (0.01);

  seg.setInputCloud (cloud);
  seg.segment (*inliers, *coefficients);

  if (inliers->indices.size () == 0)
  {
    PCL_ERROR ("Could not estimate a planar model for the given dataset.");
    return;
  }

  std::cerr << "Model inliers: " << inliers->indices.size () << std::endl;

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
  for (std::vector<int>::const_iterator pit = inliers->indices.begin (); pit != inliers->indices.end (); ++pit)
    cloud_cluster->push_back ((*cloud)[*pit]); //*
  cloud_cluster->width = cloud_cluster->size ();
  cloud_cluster->height = 1;
  cloud_cluster->is_dense = true;

  std::cout << "PointCloud representing the Cluster: " << cloud_cluster->size () << " data points." << std::endl;
  // std::stringstream ss;
  // ss << "cloud_cluster_0.pcd";
  // pcl::PCDWriter writer;
  // writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false);

  sensor_msgs::PointCloud2 output;
  pcl::toROSMsg(*cloud_cluster, output);
  output.header.frame_id = "head_camera_rgb_optical_frame";

  pub.publish (output);
}

int main(int argc, char** argv)
{
  ros::init(argc, argv, "sub_pcl");
  ros::NodeHandle nh;
  pub = nh.advertise<PointCloud> ("points2", 1);
  ros::Subscriber sub = nh.subscribe<PointCloud>("/head_camera/depth_registered/points", 1, callback);
  ros::spin();
}


#define BOOST_TEST_MODULE test_graph
#include <metaSMT/support/default_visitation_unrolling_limit.hpp>
#include <metaSMT/frontend/QF_BV.hpp>
#include <metaSMT/Graph_Context.hpp>
#include <metaSMT/support/SMT_File_Writer.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/proto/debug.hpp>
#include <boost/test/unit_test.hpp>
#include <string>

using namespace std;
using namespace metaSMT;
using namespace logic;
using namespace logic::QF_BV;
namespace proto = boost::proto;

class Graph_Fixture {
  protected:
    Graph_Context ctx;
};

BOOST_FIXTURE_TEST_SUITE(test_graph, Graph_Fixture )

BOOST_AUTO_TEST_CASE( write_smt_file )
{
  using namespace boost::assign;

  vector<SMT_Expression> assertions;
  bitvector x = new_bitvector(1);
  bitvector y = new_bitvector(1);
  bitvector z = new_bitvector(1);
  bitvector r = new_bitvector(4);

  predicate p = new_variable();

  assertions += evaluate( ctx, equal(x, bvnot(y)) );
  assertions += evaluate( ctx, equal(z, bvnot(bvnot(z))) );
  assertions += evaluate( ctx, equal(x, z) );
  assertions += evaluate( ctx, nequal(x, y) );
  assertions += evaluate( ctx, equal(bit1, bit1) );
  assertions += evaluate( ctx, equal(bit0, bit0) );

  assertions += evaluate( ctx, equal(bvand(x, x), bvand(x, x)) );
  assertions += evaluate( ctx, equal(bvor(x, x), bvor(x, x)) );
  assertions += evaluate( ctx, equal(bvnand(x, x), bvnand(x, x)) );
  assertions += evaluate( ctx, equal(bvnor(x, x), bvnor(x, x)) );
  assertions += evaluate( ctx, equal(bvxor(x, x), bvxor(x, x)) );
  assertions += evaluate( ctx, equal(bvxnor(x, x), bvxnor(x, x)) );

  assertions += evaluate( ctx, equal(bvadd(x, x), bvadd(x, x)) );
  assertions += evaluate( ctx, equal(bvsub(x, x), bvsub(x, x)) );
  assertions += evaluate( ctx, equal(bvmul(x, x), bvmul(x, x)) );

  assertions += evaluate( ctx, equal(bvsle(x, x), bvsle(x, x)) );
  assertions += evaluate( ctx, equal(bvslt(x, x), bvslt(x, x)) );
  assertions += evaluate( ctx, equal(bvsge(x, x), bvsge(x, x)) );
  assertions += evaluate( ctx, equal(bvsgt(x, x), bvsgt(x, x)) );
  assertions += evaluate( ctx, equal(bvule(x, x), bvule(x, x)) );
  assertions += evaluate( ctx, equal(bvult(x, x), bvult(x, x)) );
  assertions += evaluate( ctx, equal(bvuge(x, x), bvuge(x, x)) );
  assertions += evaluate( ctx, equal(bvugt(x, x), bvugt(x, x)) );

  assertions += evaluate( ctx, equal( bit1, bvcomp(x, x) ));
  assertions += evaluate( ctx, implies(p, p) );
  assertions += evaluate( ctx, equal(And(p, p), And(p, p)) );
  assertions += evaluate( ctx, equal(Or(p, p), Or(p, p)) );
  assertions += evaluate( ctx, equal(Xor(p, p), Xor(p, p)) );
  assertions += evaluate( ctx, equal(Nand(p, p), Nand(p, p)) );
  assertions += evaluate( ctx, equal(Nor(p, p), Nor(p, p)) );
  assertions += evaluate( ctx, equal(Xnor(p, p), Xnor(p, p)) );
  assertions += evaluate( ctx, equal(p, True) );
  assertions += evaluate( ctx, nequal(p, False) );
  assertions += evaluate( ctx, nequal(p, Not(p)) );
  assertions += evaluate( ctx, Ite(p, equal(x, x), equal(x, y)));

  assertions += evaluate( ctx, equal(bvint(10,5), bvint(10,5)) );
  assertions += evaluate( ctx, equal(r, bvsint(-2,4)) );
  assertions += evaluate( ctx, equal(bvhex("FF"), bvhex("FF")) );
  assertions += evaluate( ctx, equal(bvbin("1"), bvbin("1")) );
  assertions += evaluate( ctx, equal(bvbin("101"), bvbin("101")) );

  std::ofstream file("test.smt");
  write_smt(file, ctx.graph(), assertions);
}

BOOST_AUTO_TEST_CASE( syntax )
{
  bitvector x = new_bitvector(1);
  evaluate(ctx, x);
  evaluate(ctx, bvnot(x) );
  evaluate(ctx, bvnot(bvnot(x)) );
  evaluate(ctx, x);
  evaluate(ctx, bit1);
  evaluate(ctx, bit1);
  evaluate(ctx, bvnot(bit0));
  evaluate(ctx, bvint ( 1, 1));
  evaluate(ctx, bvint ( 0, 1));
  evaluate(ctx, bvuint( 1, 1));
  evaluate(ctx, bvuint( 0, 1));
  evaluate(ctx, bvsint(-1, 1));
  evaluate(ctx, bvsint( 0, 1));
  evaluate(ctx, bvbin ("1")  );
  evaluate(ctx, bvbin ("0") );
  evaluate(ctx, bvbin ("0") );
  evaluate(ctx, bvnand( bit0, bit1 ) );
  evaluate(ctx, bvnot(bvnand( bit0, bit1 )) );
  evaluate(ctx, bvxor(bvnand( bit0, bit1 ), bit1) );

  SMT_Expression e = evaluate( ctx, bvxor(bit1, bit1) );
  evaluate ( ctx, bvnot(e) );
  evaluate( ctx, equal(bit1, bit1) );
  evaluate( ctx, nequal(bit1, x) );

}

BOOST_AUTO_TEST_SUITE_END() //QF_BV

//  vim: ft=cpp:ts=2:sw=2:expandtab


// Copyright 2010-2014, D. E. Shaw Research.
// 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 "concepts.hpp"
#include <boost/cstdint.hpp>
#include <string>
#include <sstream>
#include "rangeIO.hpp"
#include "printlogarray.hpp"

#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>

template <typename Prf>
void dokat(const std::string& s){
    std::istringstream iss(s);
    typename Prf::domain_type ctr;
    typename Prf::key_type key;
    typename Prf::range_type answer;
    iss>>std::hex;
    iss>>rangeExtractor(ctr.begin(), ctr.end());
    iss>>rangeExtractor(key.begin(), key.end());
    iss>>rangeExtractor(answer.begin(), answer.end());
    typename Prf::range_type computed = Prf(key)(ctr);
    BOOST_CHECK_EQUAL(computed, answer);
    Prf prf(key);
    BOOST_CHECK_EQUAL(prf(ctr), answer);
}


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

    @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_FUNCTION_FIRSTBITSET_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FIRSTBITSET_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-bitwise
    This function object returns the bit pattern in which the only bit set is
    the first bit set (beginning with the least significant bit) in the parameter.

    @see  ffs,  firstbitunset

    @par Header <boost/simd/function/firstbitset.hpp>

    @par Example:

      @snippet firstbitset.cpp firstbitset

    @par Possible output:

      @snippet firstbitset.txt firstbitset

  **/
  as_integer_t<Value> firstbitset(Value const& x);
} }
#endif

#include <boost/simd/function/scalar/firstbitset.hpp>
#include <boost/simd/function/simd/firstbitset.hpp>

#endif


#include "client.hpp"
#include "routing.hpp"
#include "stats.hpp"
#include "utils.hpp"

#include <boost/range.hpp>

#include <cmath>
#include <iostream>
#include <sstream>

using namespace std;

// The singleton of the class.  The compiler initializes it to null.
stats *stats::singleton;

stats::stats(const cli_args &args, const traffic &tra):
  m_args(args), m_tra(tra),
  m_dt((args.sim_time - args.kickoff) / nom)
{
  assert(!singleton);
  singleton = this;

  // We start collecting the utilization stats at the kickoff time.
  schedule(args.kickoff);
}

template <typename T>
void
output(const string &txt, const T &v)
{
  cout << txt << " " << v << endl;
}

stats::~stats()
{
  // The population name.
  output("population", m_args.population);

  // The network utilization.
  output("utilization", ba::mean(m_utilization));

  // The algorithm statistics.
  for (const auto &e: m_bp)
    {
      // The routing type.
      auto rt = e.first;
      const string prefix = routing::to_string(rt) + '_';

      // The number of searches.
      output(prefix + "searches", m_searches[rt]);

      // The blocking probability.
      output(prefix + "bp", ba::mean(m_bp[rt]));
      // The bitrate blocking probability.
      output(prefix + "bbp",
             ba::sum(m_bb[rt]) / static_cast<double>(ba::sum(m_rb[rt])));
      // The mean cost of an established connection.
      output(prefix + "cstec", ba::mean(m_cstec[rt]));

      output(prefix + "mean_time", ba::mean(m_t[rt]));
      output(prefix + "max_time", ba::max(m_t[rt]));
      output(prefix + "mean_mmwu", ba::mean(m_mmwus[rt]));
      output(prefix + "max_mmwu", ba::max(m_mmwus[rt]));
      output(prefix + "mean_mpqc", ba::mean(m_mpqcs[rt]));
      output(prefix + "max_mpqc", ba::max(m_mpqcs[rt]));
      output(prefix + "mean_msc", ba::mean(m_mscs[rt]));
      output(prefix + "max_msc", ba::max(m_mscs[rt]));
      output(prefix + "mean_mqc", ba::mean(m_mqcs[rt]));
      output(prefix + "max_mqc", ba::max(m_mqcs[rt]));
    }

  // The number of currently active connections.
  output("conns", ba::mean(m_conns));
  // The capacity served.
  output("capser", ba::mean(m_capser));
  // The mean number of fragments on links.
  output("frags", ba::mean(m_frags));
}

stats &
stats::get()
{
  return *singleton;
}

void
stats::operator()(const double st)
{
  // The current network utilization.
  m_utilization(calculate_utilization(m_mdl));
  // The number of connections served.
  m_conns(m_tra.nr_clients());
  // The capacity served.
  m_capser(m_tra.capacity_served());
  // The number of fragments.
  m_frags(calculate_frags());

  schedule(st);
}

// Schedule the next event based on the current time 0.
void
stats::schedule(const double t)
{
  // We call the stats every second.
  module::schedule(t + m_dt);
}

void
stats::algo_perf(const routing::rt_t rt, const double dt,
                 const int &ncu,
                 const pair<array<unsigned long, 4>,
                            optional<cupp>> &p)
{
  if (m_args.kickoff <= now())
    {
      ++m_searches[rt];

      bool status = static_cast<bool>(p.second);
      m_bp[rt](!status);

      // Record the requested bitrate.
      m_rb[rt](ncu);

      if (status)
        m_cstec[rt](get_cost(m_mdl, p.second.value()));
      else
        // Record the blocked bitrate.
        m_bb[rt](ncu);

      const int mmwu = p.first[0];
      const int mpqc = p.first[1];
      const int msc = p.first[2];
      const int mqc = p.first[3];

      m_t[rt](dt);
      m_mmwus[rt](mmwu);
      m_mpqcs[rt](mpqc);
      m_mscs[rt](msc);
      m_mqcs[rt](mqc);
    }
}

double
stats::calculate_frags()
{
  dbl_acc frags;

  // Iterate over all edges.
  graph::edge_iterator ei, ee;
  for (tie(ei, ee) = boost::edges(m_mdl); ei != ee; ++ei)
    {
      const edge e = *ei;
      const SU &su = boost::get(boost::edge_su, m_mdl, e);
      int f = su.size();
      frags(f);
    }

  return ba::mean(frags);
}


#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <vector>
#include <iostream>
#include <fstream>
#include <map>
#include <cmath>

int main(int argc, char * argv[])
{
  std::string S_fname, X_fname, out_fname;

  namespace po = boost::program_options;

  po::options_description desc("Options:");
  desc.add_options()
    ("help", "help message")
    ("standard", po::value<std::string>(&S_fname)->required(), "Histogram file for null hypothesis")
    ("experiment", po::value<std::string>(&X_fname)->required(), "Histogram file for experimental result")
    ("out", po::value<std::string>(&out_fname)->required(), "Output file to contain PDF, CDF, pdf(X)/pdf(S)");

  po::positional_options_description pos_opts ;
  pos_opts.add("standard", 1);
  pos_opts.add("experiment", 1);
  pos_opts.add("out", 1);

  po::variables_map vm;

  std::string what_am_i("Calculate pdf, cdf, and conditional probability from normative and experimental histograms");

  try {
    po::store(po::command_line_parser(argc, argv).options(desc)
	      .positional(pos_opts).run(), vm);

    if(vm.count("help")) {
      std::cout << what_am_i
		<< desc << std::endl;
      exit(-1);
    }

    po::notify(vm);
  }
  catch(po::required_option & e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    std::cerr << what_am_i
	      << desc << std::endl;
    exit(-1);
  }
  catch(po::error & e) {
    std::cerr << "ERROR: " << e.what() << std::endl << std::endl;
    std::cerr << what_am_i
	      << desc << std::endl;
    exit(-1);
  }

  std::ifstream Sst(S_fname);
  std::ifstream Xst(X_fname);

  std::vector<int> S;
  std::vector<int> X;

  for(int n; Sst >> n; ) {
    S.push_back(n);
  }
  for(int n; Xst >> n; ) {
    X.push_back(n);
  }

  std::ofstream out(out_fname);

  int x_sum, s_sum;
  x_sum = s_sum = 0;
  int i;
  for(i = 0; i < S.size(); i++) {
    s_sum += S[i];
    x_sum += X[i];
  }

  // now build the pdf, cdf;
  float r_x_sum = 1.0 / ((float) x_sum);
  float r_s_sum = 1.0 / ((float) s_sum);

  float x_cdf = 0.0;
  float s_cdf = 0.0;
  for(i = 0; i < S.size(); i++) {
    float s = ((float) S[i]) * r_s_sum;
    float x = ((float) X[i]) * r_x_sum;

    s_cdf += s;
    x_cdf += x;
    out << boost::format("%d %f %f %f %f %f\n")
      % i % s % s_cdf % x % x_cdf % (x / s);
  }

  out.close();
}