#ifndef __LASER_PROC_HH__
#define __LASER_PROC_HH__

#include <sensor_msgs/Imu.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/PointCloud.h>
#include <sensor_msgs/PointCloud2.h>
#include <nav_msgs/Odometry.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <cmath>
#include <limits>
#include <vector>
#include <cassert>
#include <iostream>

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

#include <utils.hh>
#include <calib_params.hh>

#include <opencv2/opencv.hpp>

/*
   This class defines a set of 2D laser scanner filtering and
   processing functions such as
   -- noise removal, {median filter}
   -- line and corner extration, {rate_linearity}
   -- general feature extraction, {line extraction}
   -- clustering, {see below, cluster according to spatial properties}
   -- projection, {transform and projection}
   -- splitting source into multiple clusters, {see masking}
   -- labeling, {see clustering, masking}
   -- covariance estimation (as in Censi's ICRA07 paper)
   -- downsampling, {see below}
   -- generate RVIZ visualizable messages, {see below}
   -- transform data from sensor to robot/world frame, {transform}
   -- apply general transformations, {see transform}
   -- coordinate transformations. {polar2euclidean}
   This class can be utilized inside a specific node or the wrapper
   application node can be triggered to publish the data. The data
   published in the latter case can be used by multiple nodes, hence
   prevent redundant processing.

   The set of functions/filters affect cumulatively on the registered data.

   This class does book-keeping for the in/valid rays as a function of the
   provided angle spans. The valid angle spans are defined inside the
   LidarParams structure. '_mask' vector encodes the validity of each ray.
 */

using namespace std;

class LaserProc{
  private:
    double _angle_min, _angle_max,
           _angle_increment,
           _sec, _nsec;                // same as the ROS LaserScan message fields.
    vector<double> _intensities;		// ''
    vector<double> _ranges;             // stores the ranges. Overwritten if necessary.
    vector<double> _thetas;				// stores the array of angles corresponding to each range value.
    vector<double> _temp_ranges;        // used in median filter and other function where the original
    // data has to be kept unmodified till the end of the process, filtering.
    LidarCalibParams _params;           // saves a copy of the parameter set.
    vector<Eigen::Vector3d> _3d_rays;   // saves the transformed 3D point set.
    vector<Eigen::Vector2d> _2d_rays;   // saves the 2D Euclidean coordinates of the ray tips in sensor frame.
    vector<int> _mask;                  // encodes validity of range data saved in '_ranges'.
    // Also saves the cluster id's.
    vector<double> _linearity;          // encodes whether a point belongs to a line or a corner.
    // '0' means line and '1' corner.
    vector<pair<int, int> > _line_idxs; // saves the indices of the end points of lines.
    Eigen::Matrix3d _fim;               // Fisher Information Matrix.
    int _num_clusters;                  // # of clusters starting from 0.

    vector<double> _alphas;             // angles used to calculate fim in 'get_fim(...)'.

    // This stores a set of distinctive colors for RVIZ visualization.
    vector<Eigen::Vector3i> _colors;

    inline void _generate_colors(){
      int temp_colors[] = {
        0x000000, 0x00FF00, 0x0000FF, 0xFF0000,
        0x01FFFE, 0xFFA6FE, 0xFFDB66, 0x006401,
        0x010067, 0x95003A, 0x007DB5, 0xFF00F6,
        0xFFEEE8, 0x774D00, 0x90FB92, 0x0076FF,
        0xD5FF00, 0xFF937E, 0x6A826C, 0xFF029D,
        0xFE8900, 0x7A4782, 0x7E2DD2, 0x85A900,
        0xFF0056, 0xA42400, 0x00AE7E, 0x683D3B,
        0xBDC6FF, 0x263400, 0xBDD393, 0x00B917,
        0x9E008E, 0x001544, 0xC28C9F, 0xFF74A3,
        0x01D0FF, 0x004754, 0xE56FFE, 0x788231,
        0x0E4CA1, 0x91D0CB, 0xBE9970, 0x968AE8,
        0xBB8800, 0x43002C, 0xDEFF74, 0x00FFC6,
        0xFFE502, 0x620E00, 0x008F9C, 0x98FF52,
        0x7544B1, 0xB500FF, 0x00FF78, 0xFF6E41,
        0x005F39, 0x6B6882, 0x5FAD4E, 0xA75740,
        0xA5FFD2, 0xFFB167, 0x009BFF, 0xE85EBE};
      _colors.clear();
      _colors.reserve(64);
      for(int i = 0 ; i < 64 ; i++){
        _colors[i](0) = (temp_colors[i] & 0xFF0000) >> 16;
        _colors[i](1) = (temp_colors[i] & 0x00FF00) >> 8;
        _colors[i](2) = (temp_colors[i] & 0x0000FF);
      }
    }

    // These save whether '_3d_rays' and '_2d_rays' vectors needs
    // to be recalculated.
    bool _2d_euclidean_coords_valid;
    bool _3d_euclidean_coords_valid;

    // This function initializes the internal vectors and parameters.
    // Whenever new data arrives, this function is called.
    void _initialize();
    // These functions transformes the range data to their 2D/3D Euclidean
    // representations.
    void _polar_to_2d_euclidean();
    void _polar_to_3d_euclidean();

  public:
    LaserProc();
    // This function updates the internal raw copy of the lidar scan and
    // sets the internal parameters such as min/max range, dead regions etc.
    // '_mask' is initialized according to the data size and the given parameter set
    // such that it excludes short/long/inf/NaN measurements and dead regions.
    // Also rays redirected upwards and downwards are labeled as '-1' and '-2'
    // in the '_mask' array. These clusters are treated specially in many of the
    // other porcesses and filters.
    // Returns a constant reference to '_mask'.
    const vector<int>& update_data(const sensor_msgs::LaserScan &data, const LidarCalibParams &params);
    // This function is the same as the above function except this does
    // not update the paramters. This function can be used if the
    // same device as the previous frame is used.
    // Returns a constant reference to '_mask'.
    const vector<int>& update_data(const sensor_msgs::LaserScan &data);
    // This function applies median filter on the range data.
    // 'win_size' determines the size of the filter window.
    // Invalid ranges are not included in the process.
    // Returns a constant reference to the ranges vector.
    const vector<double>& median_filter(int win_size = 5);
    // This function masks out laser beams with intensity lower
    // than 'intensity_thres'. This filter can be useful when
    // working in environment which have low-reflectance surfaces
    // and fail the tailed estimators.
    const vector<int>& intensity_filter(double intensity_thres = 1000);
    // This function masks out the points those are greater (smaller) than
    // 'linearity_thres' if 'upper_bound == true' ('upper_bound == false').
    // This returns a constant reference to the resultant '_mask' vector.
    // The programmer is assumed to have called  'rate_linearity()' before
    // this function is called.
    const vector<int>& linearity_filter(double linearity_thres = 0.3, bool upper_bound = true);
    // This function removes rays such that no two ray tips are closer
    // than 'range_thres' meters to each other.
    // Returns a constant reference to '_mask'.
    const vector<int>& downsample(double range_thres = 0.05);
    // This function checks for significant jumpes on the ranges.
    // If consecutive rays differ in their ranges significantly,
    // the farther point is assumed to be occluded
    // (See LOAM paper Zhang, Singh). If 'win_size > 1' then
    // 'win_size - 1' number of neighbors of the center point are
    // marked as invalid as well. 'range_thres' is the min. difference
    // between range values of consecutive rays. It is recommended to run
    // occusion filter before any other filters. This function
    // returns a constant reference to '_mask'.
    const vector<int>& remove_occlusions(double range_thres = 0.50, int win_size = 3);
    // This function removes points on edges hit with large
    // angle of attack where angle of attack is the relative orinetation
    // between the normal of the corresponding edge and the ray in consideration.
    // All rays those have greater attack angles than 'angle_thres' are
    // removed together with the 'win_size' number of neighbors from
    // each side. This function returns a constant reference to the
    // '_mask' vector. (1.4835 rad. ~ 85 deg.)
    const vector<int>& remove_slant_edges(double angle_thres = 1.4835, int win_size = 3);
    // This function clusters the laser data. Each ray label is stored in 'mask'.
    // Clustering is carried accoring to a set of parameters:
    // -- _mask         : holds the cluster id's for all the data. '_mask[] = 0' means the ray is invalid.
    // -- _num_clusters : the number of clusters.
    // -- min_skips    : consecutive this many invalid rays cause initialization of a new cluster
    // -- range_jump   : a change in range value between consecutive rays causes init. of a new cluster
    // -- min_cluster_size : rays belonging to a cluster with number of elements less than this are marked as unused (=0)
    // Rays with 'mask[] == 0' are never assigned to another cluster.
    // Returns a constant reference to '_mask'.
    const vector<int>& cluster(int min_skips = 3, double range_jump = 0.50, int min_cluster_size = 3);
    // This function rates all of the points between [0-inf) where greater values mean a corner
    // and '0' a line. 'win_size' is the number of rays on each side of ray of concern
    // in linearity rating. Invalid points are not included in the calculation.
    // Thus, this function behaves different depending on the order of applied filters.
    // Returns a constant reference to '_linearity'.
    // The return array is of the same as the '_mask' array.
    const vector<double>& rate_linearity(int win_size = 5);
    // This function uses the built-in OpenCV 'approxPolyDP' function to fit lines.
    // Previous calls to filtering functions have direct effect on the output of this
    // function. This is because only valid points are included in the process.
    // It is guaranteed that no line passes from an invalid point.
    // The resultant set of lines with lenght '< min_len' and with number of points
    // '< min_pts' are removed from the line list. 'epsilon' is the naximum distance
    // between the original curve and its approximation (defn. from OpenCV doc.).
    // Previous call to 'rate_linerity()' does not have any effect on the output.
    // Returns a constant reference to the vector of the end point indices of the lines.
    // Other getter functions have to be used in order to find out the 2/3D/range correspondences
    // of each end point.
    const vector<pair<int, int> >& extract_lines(double min_len = 0.10, double min_pts = 3, double epsilon = 0.08);
    // This function transforms points from sensor coordinate frame to the
    // new frame given by the SE(3) object 'trans'. If 'is_sensor_pose = true'
    // then the 'trans' defines a mapping from the sensor frame to the new (world)
    // frame. Otherwise, 'trans' is interpreted as the pose of the robot. In the
    // latter case relative pose of the sensor w.r.t. the robot is integrated
    // too. The relative pose is given through the parameters structure.
    // Any filter applied after a transformation will not update the set of
    // 3D points. In order to reflect the effect, a second call to this function
    // is required. This also returns a reference to the transformed 3D points.
    // The return array is of the same as the '_mask' array.
    const vector<Eigen::Vector3d>& transform(const Eigen::Matrix4d &trans, bool is_sensor_pose);
    // This function projects points onto a plane with the normal vector 'n'.
    // Previously applied transformations do have affect on the projected point
    // set. This also returns are reference to the projected 2D points.
    // The returned array is of the same size as the '_mask' array.
    // 'project(...)' function is treated as a transformation and its results
    // are stored in '_3d_rays' vector. Hence projections can be retreived
    // using the 'get_3d_points(...)' function.
    const vector<Eigen::Vector3d>& project(const Eigen::Vector3d &n = Eigen::Vector3d(0, 0, 1));
    // This function returns a constant reference to the ranges array.
    // The vector is of the same size a the '_mask' array.
    const vector<double>& get_ranges() const;
    // This function returns the angle coordinates of the
    // ranges. This is mostly used with other functions suchs as
    // utils::laser::register(...). As long as the laser source
    // does not chance, angles are not re-calculated hence does not
    // cause extra load.
    const vector<double>& get_thetas() const;
    // This function returns the Fisher Information Matrix calculated
    // using the method given in Andrea Censi's ICRA07 paper. This takes
    // into account of the latest clusters/mask vectors.
    const Eigen::Matrix3d& estimate_fim(double skip_dist = 0.30, int skip_idxs = 30);
    // This returns FIM estimated in 'estimate_fim(...)'
    const Eigen::Matrix3d& get_fim() const;
    // This returns a constant reference to the internal '_mask' array.
    const vector<int>& get_mask() const;
    // This returns the transformed (if any) set of 3D points. The array
    // is of the same size as the '_mask' array.
    const vector<Eigen::Vector3d>& get_3d_points() const;
    // This returns the result of 'rate_linearity()' function. The returned
    // vector is of the same size as the '_mask' array.
    const vector<double>& get_linearity_rates() const;
    // This function returns a MarkerArray. Each Marker is populated with
    // the points from one cluster only. The 'i^th' marker has the points
    // from the 'i^th' cluster. The last four markers have all the
    // points, linearity rates, line segments and the covariance estimate
    // respectively. The set of points reflect the effect of previous
    // transformations.
    bool get_RVIZ_markers(visualization_msgs::MarkerArray &marray) const;
    // This returns the extrated line indices.
    const vector<pair<int, int> >& get_line_indexes() const;
    // This returns the calibration parameters structure
    const LidarCalibParams& get_calib_params() const {
      return _params;
    }
    // This returns the timestamp of the original laser scan.
    ros::Time get_timestamp() const {
      return ros::Time(_sec, _nsec);
    }
    // This returns the number of clusters
    int get_num_clusters() const{
      return _num_clusters;
    }
    double get_angle_min() const { return _angle_min;}
    double get_angle_max() const { return _angle_max;}
    double get_angle_increment() const { return _angle_increment;}
};

#endif


#include <fstream>
#include <iostream>

#include <boost/functional/hash.hpp>
#include <boost/program_options.hpp>

#include <yaml-cpp/yaml.h>

#include <libMultiRobotPlanning/cbs_tree.hpp>

#include <libMultiRobotPlanning/cbs.hpp>
#include "timer.hpp"
#include <libMultiRobotPlanning/store_tree_cbs.hpp>

using libMultiRobotPlanning::CBS;
using libMultiRobotPlanning::TREE_CBS;
using libMultiRobotPlanning::treeNode;
using libMultiRobotPlanning::btree;
using libMultiRobotPlanning::Neighbor;
using libMultiRobotPlanning::PlanResult;

struct State {
  State(int time, int x, int y) : time(time), x(x), y(y) {}

  bool operator==(const State& s) const {
    return time == s.time && x == s.x && y == s.y;
  }

  bool equalExceptTime(const State& s) const { return x == s.x && y == s.y; }

  friend std::ostream& operator<<(std::ostream& os, const State& s) {
    return os << s.time << ": (" << s.x << "," << s.y << ")";
    // return os << "(" << s.x << "," << s.y << ")";
  }

  int time;
  int x;
  int y;
};

namespace std {
template <>
struct hash<State> {
  size_t operator()(const State& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.time);
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    return seed;
  }
};
}  // namespace std

///
enum class Action {
  Up,
  Down,
  Left,
  Right,
  Wait,
};

std::ostream& operator<<(std::ostream& os, const Action& a) {
  switch (a) {
    case Action::Up:
      os << "Up";
      break;
    case Action::Down:
      os << "Down";
      break;
    case Action::Left:
      os << "Left";
      break;
    case Action::Right:
      os << "Right";
      break;
    case Action::Wait:
      os << "Wait";
      break;
  }
  return os;
}

///

struct Conflict {
  enum Type {
    Vertex,
    Edge,
  };

  int time;
  size_t agent1;
  size_t agent2;
  Type type;

  int x1;
  int y1;
  int x2;
  int y2;

  friend std::ostream& operator<<(std::ostream& os, const Conflict& c) {
    switch (c.type) {
      case Vertex:
        return os << c.time << ": Vertex(" << c.x1 << "," << c.y1 << ")";
      case Edge:
        return os << c.time << ": Edge(" << c.x1 << "," << c.y1 << "," << c.x2
                  << "," << c.y2 << ")";
    }
    return os;
  }
};

struct VertexConstraint {
  VertexConstraint(int time, int x, int y) : time(time), x(x), y(y) {}
  int time;
  int x;
  int y;

  bool operator<(const VertexConstraint& other) const {
    return std::tie(time, x, y) < std::tie(other.time, other.x, other.y);
  }

  bool operator==(const VertexConstraint& other) const {
    return std::tie(time, x, y) == std::tie(other.time, other.x, other.y);
  }

  friend std::ostream& operator<<(std::ostream& os, const VertexConstraint& c) {
    return os << "VC(" << c.time << "," << c.x << "," << c.y << ")";
  }
};

namespace std {
template <>
struct hash<VertexConstraint> {
  size_t operator()(const VertexConstraint& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.time);
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    return seed;
  }
};
}  // namespace std

struct EdgeConstraint {
  EdgeConstraint(int time, int x1, int y1, int x2, int y2)
      : time(time), x1(x1), y1(y1), x2(x2), y2(y2) {}
  int time;
  int x1;
  int y1;
  int x2;
  int y2;

  bool operator<(const EdgeConstraint& other) const {
    return std::tie(time, x1, y1, x2, y2) <
           std::tie(other.time, other.x1, other.y1, other.x2, other.y2);
  }

  bool operator==(const EdgeConstraint& other) const {
    return std::tie(time, x1, y1, x2, y2) ==
           std::tie(other.time, other.x1, other.y1, other.x2, other.y2);
  }

  friend std::ostream& operator<<(std::ostream& os, const EdgeConstraint& c) {
    return os << "EC(" << c.time << "," << c.x1 << "," << c.y1 << "," << c.x2
              << "," << c.y2 << ")";
  }
};

namespace std {
template <>
struct hash<EdgeConstraint> {
  size_t operator()(const EdgeConstraint& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.time);
    boost::hash_combine(seed, s.x1);
    boost::hash_combine(seed, s.y1);
    boost::hash_combine(seed, s.x2);
    boost::hash_combine(seed, s.y2);
    return seed;
  }
};
}  // namespace std

struct Constraints {
  std::set<VertexConstraint> vertexConstraints;
  std::set<EdgeConstraint> edgeConstraints;

  void add(const Constraints& other) {
    vertexConstraints.insert(other.vertexConstraints.begin(),
                             other.vertexConstraints.end());
    edgeConstraints.insert(other.edgeConstraints.begin(),
                           other.edgeConstraints.end());
  }

  bool overlap(const Constraints& other) {
    std::vector<VertexConstraint> vertexIntersection;
    std::vector<EdgeConstraint> edgeIntersection;
    std::set_intersection(vertexConstraints.begin(), vertexConstraints.end(),
                          other.vertexConstraints.begin(),
                          other.vertexConstraints.end(),
                          std::back_inserter(vertexIntersection));
    std::set_intersection(edgeConstraints.begin(), edgeConstraints.end(),
                          other.edgeConstraints.begin(),
                          other.edgeConstraints.end(),
                          std::back_inserter(edgeIntersection));
    return !vertexIntersection.empty() || !edgeIntersection.empty();
  }

  friend std::ostream& operator<<(std::ostream& os, const Constraints& c) {
    for (const auto& vc : c.vertexConstraints) {
      os << vc << std::endl;
    }
    for (const auto& ec : c.edgeConstraints) {
      os << ec << std::endl;
    }
    return os;
  }
  //my:
  friend bool operator==(const Constraints& l, const Constraints& r) {
    if (l.edgeConstraints == r.edgeConstraints){
      if(l.vertexConstraints == r.vertexConstraints){
        return true;
      }
    }
    return false;
  }
  //my:
  friend bool operator<(const Constraints& l, const Constraints& r) {
    if (l.vertexConstraints < r.vertexConstraints){
      return true;
    }else if(l.vertexConstraints == r.vertexConstraints){
      if (l.edgeConstraints < r.edgeConstraints){
        return true;
      }
    }
    return false;
  }

};

struct Location {
  Location(int x, int y) : x(x), y(y) {}
  int x;
  int y;

  bool operator<(const Location& other) const {
    return std::tie(x, y) < std::tie(other.x, other.y);
  }

  bool operator==(const Location& other) const {
    return std::tie(x, y) == std::tie(other.x, other.y);
  }

  friend std::ostream& operator<<(std::ostream& os, const Location& c) {
    return os << "(" << c.x << "," << c.y << ")";
  }
};

namespace std {
template <>
struct hash<Location> {
  size_t operator()(const Location& s) const {
    size_t seed = 0;
    boost::hash_combine(seed, s.x);
    boost::hash_combine(seed, s.y);
    return seed;
  }
};
}  // namespace std

///
class Environment {
 public:
  Environment(size_t dimx, size_t dimy, std::unordered_set<Location> obstacles,
              std::vector<Location> goals)
      : m_dimx(dimx),
        m_dimy(dimy),
        m_obstacles(std::move(obstacles)),
        m_goals(std::move(goals)),
        m_agentIdx(0),
        m_constraints(nullptr),
        m_lastGoalConstraint(-1),
        m_highLevelExpanded(0),
        m_lowLevelExpanded(0) {
    // computeHeuristic();
  }

  Environment(const Environment&) = delete;
  Environment& operator=(const Environment&) = delete;

  void setLowLevelContext(size_t agentIdx, const Constraints* constraints) {
    assert(constraints);  // NOLINT
    m_agentIdx = agentIdx;
    m_constraints = constraints;
    m_lastGoalConstraint = -1;
    for (const auto& vc : constraints->vertexConstraints) {
      if (vc.x == m_goals[m_agentIdx].x && vc.y == m_goals[m_agentIdx].y) {
        m_lastGoalConstraint = std::max(m_lastGoalConstraint, vc.time);
      }
    }
  }

  int admissibleHeuristic(const State& s) {
    // std::cout << "H: " <<  s << " " << m_heuristic[m_agentIdx][s.x + m_dimx *
    // s.y] << std::endl;
    // return m_heuristic[m_agentIdx][s.x + m_dimx * s.y];
    return std::abs(s.x - m_goals[m_agentIdx].x) +
           std::abs(s.y - m_goals[m_agentIdx].y);
  }

  bool isSolution(const State& s) {
    return s.x == m_goals[m_agentIdx].x && s.y == m_goals[m_agentIdx].y &&
           s.time > m_lastGoalConstraint;
  }

  void getNeighbors(const State& s,
                    std::vector<Neighbor<State, Action, int> >& neighbors) {
    // std::cout << "#VC " << constraints.vertexConstraints.size() << std::endl;
    // for(const auto& vc : constraints.vertexConstraints) {
    //   std::cout << "  " << vc.time << "," << vc.x << "," << vc.y <<
    //   std::endl;
    // }
    neighbors.clear();
    {
      State n(s.time + 1, s.x, s.y);
      if (stateValid(n) && transitionValid(s, n)) {
        neighbors.emplace_back(
            Neighbor<State, Action, int>(n, Action::Wait, 1));
      }
    }
    {
      State n(s.time + 1, s.x - 1, s.y);
      if (stateValid(n) && transitionValid(s, n)) {
        neighbors.emplace_back(
            Neighbor<State, Action, int>(n, Action::Left, 1));
      }
    }
    {
      State n(s.time + 1, s.x + 1, s.y);
      if (stateValid(n) && transitionValid(s, n)) {
        neighbors.emplace_back(
            Neighbor<State, Action, int>(n, Action::Right, 1));
      }
    }
    {
      State n(s.time + 1, s.x, s.y + 1);
      if (stateValid(n) && transitionValid(s, n)) {
        neighbors.emplace_back(Neighbor<State, Action, int>(n, Action::Up, 1));
      }
    }
    {
      State n(s.time + 1, s.x, s.y - 1);
      if (stateValid(n) && transitionValid(s, n)) {
        neighbors.emplace_back(
            Neighbor<State, Action, int>(n, Action::Down, 1));
      }
    }
  }

  bool getFirstConflict(
      const std::vector<PlanResult<State, Action, int> >& solution,
      Conflict& result) {
    int max_t = 0;
    for (const auto& sol : solution) {
      max_t = std::max<int>(max_t, sol.states.size() - 1);
    }

    for (int t = 0; t < max_t; ++t) {
      // check drive-drive vertex collisions
      for (size_t i = 0; i < solution.size(); ++i) {
        State state1 = getState(i, solution, t);
        for (size_t j = i + 1; j < solution.size(); ++j) {
          State state2 = getState(j, solution, t);
          if (state1.equalExceptTime(state2)) {
            result.time = t;
            result.agent1 = i;
            result.agent2 = j;
            result.type = Conflict::Vertex;
            result.x1 = state1.x;
            result.y1 = state1.y;
            // std::cout << "VC " << t << "," << state1.x << "," << state1.y <<
            // std::endl;
            return true;
          }
        }
      }
      // drive-drive edge (swap)
      for (size_t i = 0; i < solution.size(); ++i) {
        State state1a = getState(i, solution, t);
        State state1b = getState(i, solution, t + 1);
        for (size_t j = i + 1; j < solution.size(); ++j) {
          State state2a = getState(j, solution, t);
          State state2b = getState(j, solution, t + 1);
          if (state1a.equalExceptTime(state2b) &&
              state1b.equalExceptTime(state2a)) {
            result.time = t;
            result.agent1 = i;
            result.agent2 = j;
            result.type = Conflict::Edge;
            result.x1 = state1a.x;
            result.y1 = state1a.y;
            result.x2 = state1b.x;
            result.y2 = state1b.y;
            return true;
          }
        }
      }
    }

    return false;
  }

  void createConstraintsFromConflict(
      const Conflict& conflict, std::map<size_t, Constraints>& constraints) {
    if (conflict.type == Conflict::Vertex) {
      Constraints c1;
      c1.vertexConstraints.emplace(
          VertexConstraint(conflict.time, conflict.x1, conflict.y1));
      constraints[conflict.agent1] = c1;
      constraints[conflict.agent2] = c1;
    } else if (conflict.type == Conflict::Edge) {
      Constraints c1;
      c1.edgeConstraints.emplace(EdgeConstraint(
          conflict.time, conflict.x1, conflict.y1, conflict.x2, conflict.y2));
      constraints[conflict.agent1] = c1;
      Constraints c2;
      c2.edgeConstraints.emplace(EdgeConstraint(
          conflict.time, conflict.x2, conflict.y2, conflict.x1, conflict.y1));
      constraints[conflict.agent2] = c2;
    }
  }

  void onExpandHighLevelNode(int /*cost*/) { m_highLevelExpanded++; }

  void onExpandLowLevelNode(const State& /*s*/, int /*fScore*/,
                            int /*gScore*/) {
    m_lowLevelExpanded++;
  }

  int highLevelExpanded() { return m_highLevelExpanded; }

  int lowLevelExpanded() const { return m_lowLevelExpanded; }

 private:
  State getState(size_t agentIdx,
                 const std::vector<PlanResult<State, Action, int> >& solution,
                 size_t t) {
    assert(agentIdx < solution.size());
    if (t < solution[agentIdx].states.size()) {
      return solution[agentIdx].states[t].first;
    }
    assert(!solution[agentIdx].states.empty());
    return solution[agentIdx].states.back().first;
  }

  bool stateValid(const State& s) {
    assert(m_constraints);
    const auto& con = m_constraints->vertexConstraints;
    return s.x >= 0 && s.x < m_dimx && s.y >= 0 && s.y < m_dimy &&
           m_obstacles.find(Location(s.x, s.y)) == m_obstacles.end() &&
           con.find(VertexConstraint(s.time, s.x, s.y)) == con.end();
  }

  bool transitionValid(const State& s1, const State& s2) {
    assert(m_constraints);
    const auto& con = m_constraints->edgeConstraints;
    return con.find(EdgeConstraint(s1.time, s1.x, s1.y, s2.x, s2.y)) ==
           con.end();
  }
#if 0
  // We use another A* search for simplicity
  // we compute the shortest path to each goal by using the fact that our getNeighbor function is
  // symmetric and by not terminating the AStar search until the queue is empty
  void computeHeuristic()
  {
    class HeuristicEnvironment
    {
    public:
      HeuristicEnvironment(
        size_t dimx,
        size_t dimy,
        const std::unordered_set<Location>& obstacles,
        std::vector<int>* heuristic)
        : m_dimx(dimx)
        , m_dimy(dimy)
        , m_obstacles(obstacles)
        , m_heuristic(heuristic)
      {
      }

      int admissibleHeuristic(
        const Location& s)
      {
        return 0;
      }

      bool isSolution(
        const Location& s)
      {
        return false;
      }

      void getNeighbors(
        const Location& s,
        std::vector<Neighbor<Location, Action, int> >& neighbors)
      {
        neighbors.clear();

        {
          Location n(s.x-1, s.y);
          if (stateValid(n)) {
            neighbors.emplace_back(Neighbor<Location, Action, int>(n, Action::Left, 1));
          }
        }
        {
          Location n(s.x+1, s.y);
          if (stateValid(n)) {
            neighbors.emplace_back(Neighbor<Location, Action, int>(n, Action::Right, 1));
          }
        }
        {
          Location n(s.x, s.y+1);
          if (stateValid(n)) {
            neighbors.emplace_back(Neighbor<Location, Action, int>(n, Action::Up, 1));
          }
        }
        {
          Location n(s.x, s.y-1);
          if (stateValid(n)) {
            neighbors.emplace_back(Neighbor<Location, Action, int>(n, Action::Down, 1));
          }
        }
      }

      void onExpandNode(
        const Location& s,
        int fScore,
        int gScore)
      {
      }

      void onDiscover(
        const Location& s,
        int fScore,
        int gScore)
      {
        (*m_heuristic)[s.x + m_dimx * s.y] = gScore;
      }

    private:
      bool stateValid(
        const Location& s)
      {
        return    s.x >= 0
               && s.x < m_dimx
               && s.y >= 0
               && s.y < m_dimy
               && m_obstacles.find(Location(s.x, s.y)) == m_obstacles.end();
      }

    private:
      int m_dimx;
      int m_dimy;
      const std::unordered_set<Location>& m_obstacles;
      std::vector<int>* m_heuristic;

    };

    m_heuristic.resize(m_goals.size());

    std::vector< Neighbor<State, Action, int> > neighbors;

    for (size_t i = 0; i < m_goals.size(); ++i) {
      m_heuristic[i].assign(m_dimx * m_dimy, std::numeric_limits<int>::max());
      HeuristicEnvironment henv(m_dimx, m_dimy, m_obstacles, &m_heuristic[i]);
      AStar<Location, Action, int, HeuristicEnvironment> astar(henv);
      PlanResult<Location, Action, int> dummy;
      astar.search(m_goals[i], dummy);
      m_heuristic[i][m_goals[i].x + m_dimx * m_goals[i].y] = 0;
    }
  }
#endif
 private:
  int m_dimx;
  int m_dimy;
  std::unordered_set<Location> m_obstacles;
  std::vector<Location> m_goals;
  // std::vector< std::vector<int> > m_heuristic;
  size_t m_agentIdx;
  const Constraints* m_constraints;
  int m_lastGoalConstraint;
  int m_highLevelExpanded;
  int m_lowLevelExpanded;
};

void WriteSolutionToOutputFile(bool success,std::vector<libMultiRobotPlanning::PlanResult<State, Action, int>> solution
,std::string outputFile, Timer timer,Environment* mapf){
  if (success) {
    std::cout << "Planning successful! " << std::endl;
    int cost = 0;
    int makespan = 0;
    for (const auto& s : solution) {
      cost += s.cost;
      makespan = std::max<int>(makespan, s.cost);
    }

    std::ofstream out(outputFile);
    out << "statistics:" << std::endl;
    out << "  cost: " << cost << std::endl;
    out << "  makespan: " << makespan << std::endl;
    out << "  runtime: " << timer.elapsedSeconds() << std::endl;
    out << "  highLevelExpanded: " << (*mapf).highLevelExpanded() << std::endl;
    out << "  lowLevelExpanded: " << (*mapf).lowLevelExpanded() << std::endl;
    out << "schedule:" << std::endl;
    for (size_t a = 0; a < solution.size(); ++a) {
      // std::cout << "Solution for: " << a << std::endl;
      // for (size_t i = 0; i < solution[a].actions.size(); ++i) {
      //   std::cout << solution[a].states[i].second << ": " <<
      //   solution[a].states[i].first << "->" << solution[a].actions[i].first
      //   << "(cost: " << solution[a].actions[i].second << ")" << std::endl;
      // }
      // std::cout << solution[a].states.back().second << ": " <<
      // solution[a].states.back().first << std::endl;

      out << "  agent" << a << ":" << std::endl;
      for (const auto& state : solution[a].states) {
        out << "    - x: " << state.first.x << std::endl
            << "      y: " << state.first.y << std::endl
            << "      t: " << state.second << std::endl;
      }
    }

  } else {
    std::cout << "Planning NOT successful!" << std::endl;
  }
}

  int UpdateNodeConstraints(std::vector<Constraints>& constraintsVector, int timeStep){
  //remove from each constraint with time >= timestep a timestep
      int total_number_of_constraints_in_node =0;
      for ( Constraints& constraint : constraintsVector){
          std::set<EdgeConstraint> newEdgeConstraints;
          for(EdgeConstraint edgeConstraint : constraint.edgeConstraints){
              if(edgeConstraint.time > timeStep){ // > : we don't care about "the" timeStep constrain (this is the start point)
                      edgeConstraint.time = edgeConstraint.time - timeStep;
                      newEdgeConstraints.insert(edgeConstraint);
              }
          }
          total_number_of_constraints_in_node += newEdgeConstraints.size();
          constraint.edgeConstraints = newEdgeConstraints;
          std::set<VertexConstraint> newVertexConstraints;
          for(VertexConstraint vertexConstraint : constraint.vertexConstraints){
              if(vertexConstraint.time > timeStep){
                  vertexConstraint.time = vertexConstraint.time - timeStep;
                  newVertexConstraints.insert(vertexConstraint);
              }
          }
          total_number_of_constraints_in_node += newVertexConstraints.size();
          constraint.vertexConstraints = newVertexConstraints;
      }
      return total_number_of_constraints_in_node;
  }
  //template<typename HighLevelNode, typename Conflict, typename State>
  bool UpdateNodes(treeNode<TREE_CBS<State, Action, int, Conflict, Constraints, Environment>::HighLevelNode,Conflict>* node, int& id, int timeStep, size_t agentNumber,std::vector<State> &startStates, bool &simple_node_with_zero_constraints_exist){
      //update id
      node->highLevelNodeTree->id = id;
      id++;
      //initialize new cost
      int newCost = 0;
      //check which nodes are relevent
      for( std::size_t i = 0; i < node->highLevelNodeTree->solution.size(); i++){
        libMultiRobotPlanning::PlanResult<State, Action, int>& sol = node->highLevelNodeTree->solution[i];
        if((int)sol.states.size() > timeStep){
          //the agent has not arrived at his goal (before or in the timestep)
          if (sol.states[timeStep].first.x != startStates[i].x || sol.states[timeStep].first.y != startStates[i].y){
            //we eant to remove all nodes from treeNodeVector that is not compatible with the chosen solution from the first phase(the start point at timeStep is not the same)
            return false;
          }
        }
      }
      // update constraints:
      int total_number_of_constraints_in_node = UpdateNodeConstraints((node->highLevelNodeTree->constraints), timeStep);
      if( 0 == total_number_of_constraints_in_node && simple_node_with_zero_constraints_exist==true){
        return false;
      }else if(0 == total_number_of_constraints_in_node){
        simple_node_with_zero_constraints_exist = true;
      }
      // update paths: (delete all places before the timeStep)
      for( std::size_t i = 0; i < node->highLevelNodeTree->solution.size(); i++){
          libMultiRobotPlanning::PlanResult<State, Action, int>& sol = node->highLevelNodeTree->solution[i];
          int solStateSize = sol.states.size();

          if (i == agentNumber){
              //the new path will be calculated in the search phase
              continue;
          }

          if(solStateSize <= timeStep){
              //the agent already arrived at his goal (before or in the timestep)
              sol.states.erase(sol.states.begin(), sol.states.begin() + solStateSize-1);
              sol.actions.erase(sol.actions.begin(), sol.actions.begin() + solStateSize-1);
              for(std::pair<State, int>& state : sol.states){
                  state.first.time= 0;
                  state.second = 0;
              }
          }else{
              sol.states.erase(sol.states.begin(), sol.states.begin() + timeStep);
              sol.actions.erase(sol.actions.begin(), sol.actions.begin() + timeStep);
              for(std::pair<State, int>& state : sol.states){
                state.first.time= state.first.time - timeStep;
                state.second = state.second - timeStep;
              }
          }
          int size = sol.states.size() - 1;
          newCost += size;
          sol.cost = size;
          std::pair<State, int>* stateEnd = sol.states.end().base();
          sol.fmin = std::abs(sol.states[0].first.x - stateEnd->first.x ) + std::abs(sol.states[0].first.y - stateEnd->first.y);
      }

      // update cost:
      node->highLevelNodeTree->cost = newCost;

      return true;
  }

int main(int argc, char* argv[]) {
  namespace po = boost::program_options;
  // Declare the supported options.
  po::options_description desc("Allowed options");
  std::string inputFile;
  // std::string outputFileBase = "/home/hadar/My_Project_MAPF/debug/outputFileBase.yaml";
  // std::string outputFileNew = "/home/hadar/My_Project_MAPF/debug/outputFileNew.yaml";
  // std::string secondOutputFileBase = "/home/hadar/My_Project_MAPF/debug/secondOutputFileBase.yaml";
  // std::string secondOutputFileNew = "/home/hadar/My_Project_MAPF/debug/secondOutputFileNew.yaml";
  std::string outputFileBase;
  std::string outputFileNew;
  std::string secondOutputFileBase;
  std::string secondOutputFileNew;
  std::string approach;
  std::size_t agentNumber;
  int timeStep;
  int goal_x;
  int goal_y;

  desc.add_options()
      ("help", "produce help message")
      ("input,i", po::value<std::string>(&inputFile)->required(),
      "input file (YAML)")
      ("outputFileBase", po::value<std::string>(&outputFileBase)->required(),
      "output file Baseline (YAML)")
      ("outputFileNew", po::value<std::string>(&outputFileNew)->required(),
      "output file new (YAML)")
      ("secondOutputFileBase", po::value<std::string>(&secondOutputFileBase)->required(),
      "second output file Baseline (YAML)")
      ("secondOutputFileNew", po::value<std::string>(&secondOutputFileNew)->required(),
      "second output file new (YAML)")
      ("approach", po::value<std::string>(&approach)->required(),
      "naive or pruning approach")
      ("agentNumber", po::value<std::size_t>(&agentNumber)->required(),
      "agent with new goal location")
      ("timeStep", po::value<int>(&timeStep)->required(),
      "the time step in which the agent goal is changing")
      ("x", po::value<int>(&goal_x)->required(),
      "location x of the new goal")
      ("y", po::value<int>(&goal_y)->required(),
      "location y of the new goal");

  try {
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    if (vm.count("help") != 0u) {
      std::cout << desc << "\n";
      return 0;
    }
  } catch (po::error& e) {
    std::cerr << e.what() << std::endl << std::endl;
    std::cerr << desc << std::endl;
    return 1;
  }

  YAML::Node config = YAML::LoadFile(inputFile);

  std::unordered_set<Location> obstacles;
  std::vector<Location> goals;
  std::vector<State> startStates;

  const auto& dim = config["map"]["dimensions"];
  int dimx = dim[0].as<int>();
  int dimy = dim[1].as<int>();

  for (const auto& node : config["map"]["obstacles"]) {
    obstacles.insert(Location(node[0].as<int>(), node[1].as<int>()));
  }

  for (const auto& node : config["agents"]) {
    const auto& start = node["start"];
    const auto& goal = node["goal"];
    startStates.emplace_back(State(0, start[0].as<int>(), start[1].as<int>()));
    // std::cout << "s: " << startStates.back() << std::endl;
    goals.emplace_back(Location(goal[0].as<int>(), goal[1].as<int>()));
  }

  //Flags checks:
  //1. Check that the goal is inside the map borders and that it is not an obstacle
  if(goal_x > dimx || goal_x < 0 || goal_y < 0|| goal_y >dimy){
    std::cout << "The new goal location is out of map borders!" << std::endl;
    return 0;
  }
  for (const auto& elem: obstacles) {
    if(elem.x == goal_x && elem.y == goal_y){
      std::cout << "The new goal location is an obstacle!" << std::endl;
      return 0;
    }
  }
  //2. The agent number is valid
  if(agentNumber > goals.size()){
    std::cout << "The agentNumber is not valid!" << std::endl;
    return 0;
  }
  //3.timestep >= 0
  if(timeStep < 0){
    std::cout << "The timeStep can not be negative!" << std::endl;
    return 0;
  }
  //4. Check that the new goal is not the same as another agent goal
  for (const auto& elem: goals){
    if(elem.x == goal_x && elem.y == goal_y){
      std::cout << "The goal location is the same as another agent goal!" << std::endl;
      return 0;
    }
  }

  // open csv file to write results
  std::ofstream myfile;
  myfile.open ("results_for_graph.csv",std::ios_base::app);
  std::ofstream myfile2;
  myfile2.open ("results_for_graph_improment_to_number_of_nodes.csv",std::ios_base::app);
  std::ofstream myfile3;
  myfile3.open ("results_for_graph_number_of_constraint_fisrt_level",std::ios_base::app);
  std::ofstream myfile4;
  myfile4.open ("results_for_graph_old_and_new_distance_to_goal",std::ios_base::app);
  std::ofstream myfile5;
  myfile5.open ("results_for_graph__distance_between_goals",std::ios_base::app);
  myfile5 << inputFile << ",";
  myfile5 << sqrt(pow((startStates[agentNumber].x -goal_x),2) + pow((startStates[agentNumber].y -goal_y),2)) << std::endl;
  myfile5.close();
  // return 0 ;

  //************first phase*************// - without saving a tree (regular run)
  // First run of CBS
  Environment mapf(dimx, dimy, obstacles, goals);
  CBS<State, Action, int, Conflict, Constraints, Environment> cbs(mapf);
  std::vector<PlanResult<State, Action, int> > solution;

  Timer timer;
  bool success = cbs.search(startStates, solution, myfile2, agentNumber, timeStep, myfile3); // for analysis: myfile2, agentNumber, timeStep
  timer.stop();
  std::cout << "base CBS: " << timer.elapsedSeconds() << std::endl;

  WriteSolutionToOutputFile(success,solution,outputFileBase,timer,&mapf);
  //************first phase*************// - without saving a tree (regular run)

  //************first phase - build CT with CBS*************//
  // First run of CBS
  Environment tree_mapf(dimx, dimy, obstacles, goals);
  TREE_CBS<State, Action, int, Conflict, Constraints, Environment> tree_cbs(tree_mapf);
  std::vector<PlanResult<State, Action, int> > tree_solution;

  btree< TREE_CBS<State, Action, int, Conflict, Constraints, Environment>::HighLevelNode, Conflict, State> *ct_tree = new btree< TREE_CBS<State, Action, int, Conflict, Constraints, Environment>::HighLevelNode, Conflict, State>();

  Timer tree_timer;
  std::vector<treeNode<TREE_CBS<State, Action, int, Conflict, Constraints, Environment>::HighLevelNode,Conflict>*> emptyVector = std::vector<treeNode<TREE_CBS<State, Action, int, Conflict, Constraints, Environment>::HighLevelNode,Conflict>*>();
  bool tree_success = tree_cbs.search(startStates, tree_solution, ct_tree, emptyVector,1,0);
  tree_timer.stop();
  std::cout << "tree CBS: " << tree_timer.elapsedSeconds() << std::endl;

  WriteSolutionToOutputFile(tree_success,tree_solution,outputFileNew,tree_timer,&tree_mapf);
  //************first phase - build CT with CBS*************//
    std::cout << "difference( tree / base): " << tree_timer.elapsedSeconds() / timer.elapsedSeconds() << std::endl;

  //************second phase- find new paths after "the agent" goal changed*************//

  // for results only:
  int distance_to_old_goal = 0;
  if ((int)(solution[agentNumber].states.size()) > timeStep){
    distance_to_old_goal = std::abs(solution[agentNumber].states[timeStep].first.x - goals[agentNumber].x ) + std::abs(solution[agentNumber].states[timeStep].first.y - goals[agentNumber].y );
  }

  //change the chosen agent goal location
  goals[agentNumber].x= goal_x;
  goals[agentNumber].y= goal_y;
  //change all agents start location using the solution from old CBS
  for (size_t a = 0; a < solution.size(); ++a) {
    if(solution[a].states.size() <= (size_t)timeStep){
      // the agent has arrived at its goal location
      if( a == agentNumber){
        //in this case, our agent already arrived at its old goal but now he needs to go to its new goal
        // Note: if I want him to remain in its old goal then no second phase needs to occur.
        startStates[a].x = solution[a].states.back().first.x;
        startStates[a].y =solution[a].states.back().first.y;
      }else{
        startStates[a].x = goals[a].x;
        startStates[a].y = goals[a].y;
      }
    }else{
    startStates[a].x = solution[a].states[timeStep].first.x;
    startStates[a].y = solution[a].states[timeStep].first.y;
    }
  }

  // for results only:
  int distance_to_new_goal = std::abs(startStates[agentNumber].x - goals[agentNumber].x ) + std::abs(startStates[agentNumber].y - goals[agentNumber].y );
  int distance_difference =  std::abs( distance_to_new_goal - distance_to_old_goal );
  myfile4 << distance_difference << std::endl;
  //if(approach == "baseline" ){
    // //make new environment and run cbs:
    Timer second_timer;
    Environment second_mapf(dimx, dimy, obstacles, goals);
    CBS<State, Action, int, Conflict, Constraints, Environment> second_cbs(second_mapf);
    std::vector<PlanResult<State, Action, int> > second_solution;

    bool second_success = second_cbs.search(startStates, second_solution, myfile2,agentNumber,timeStep,myfile3);
    myfile3 << std::endl;

    second_timer.stop();
    std::cout << second_timer.elapsedSeconds() <<std::endl;

      //Write second output file with the new paths
    WriteSolutionToOutputFile(second_success,second_solution,secondOutputFileBase,second_timer,&second_mapf);

  //}else{ // approach == "pruning"

    Timer second_tree_timer;
    //make new environment and run cbs:
    Environment second_tree_mapf(dimx, dimy, obstacles, goals);
    TREE_CBS<State, Action, int, Conflict, Constraints, Environment> second_tree_cbs(second_tree_mapf);
    std::vector<PlanResult<State, Action, int> > second_tree_solution;

    //prun OCT
    int id = 1;
    Timer check_alg_time;
    std::vector<treeNode<TREE_CBS<State, Action, int, Conflict, Constraints, Environment>::HighLevelNode,Conflict>*> treeNodeVector = ct_tree->PreorderPrunTreeTraveling(agentNumber, timeStep,id, agentNumber, startStates);
    check_alg_time.stop();
    std::cout << "time to udate all nodes in new algorithem " << check_alg_time.elapsedSeconds() << std::endl;

    if (treeNodeVector.empty()){
      ct_tree->nullToRoot();
    }
    //debuge print:
    std::cout<< "new: number of relevent nodes to re-use:" << treeNodeVector.size() <<std::endl;
    myfile2 << treeNodeVector.size() << std:: endl;

    // run CBS with the newly created Open list

    //btree< TREE_CBS<State, Action, int, Conflict, Constraints, Environment>::HighLevelNode, Conflict> *second_ct_tree = new btree< TREE_CBS<State, Action, int, Conflict, Constraints, Environment>::HighLevelNode, Conflict>();

    bool second_tree_success = second_tree_cbs.search(startStates, second_tree_solution, ct_tree, treeNodeVector, id, agentNumber);
    second_tree_timer.stop();
    std::cout << second_tree_timer.elapsedSeconds() <<std::endl;

    WriteSolutionToOutputFile(second_tree_success,second_tree_solution,secondOutputFileNew,second_tree_timer,&second_tree_mapf);
  //}
  //************second phase- find new paths after "the agent" goal changed*************//

  // write results to file
  myfile << second_timer.elapsedSeconds() << "," << second_tree_timer.elapsedSeconds() << "\n";
  myfile.close();
  myfile2.close();
  myfile3.close();
  myfile4.close();

//*************old*************
  delete ct_tree;
  return 0;
}


#include "ros/ros.h"
#include "std_msgs/String.h"
#include"Eigen/Dense"
#include <vector>
#include <iostream>
#include <QString>
#include <QList>
#include "Robot.h"
#include"taskspaceonline3.h"
#include <qmath.h>
#include <cstring>
#include<qdebug.h>
#include <Eigen/Geometry>
#include <cstdlib>
#include "Eigen/eiquadprog.h"
#include "Eigen/Core"
#include "Eigen/Cholesky"
#include "Eigen/LU"
#include<std_msgs/Int32MultiArray.h>
#include<std_msgs/Float32MultiArray.h>
#include<geometry_msgs/Wrench.h>
#include<math.h>
#include<sensor_msgs/Imu.h>
#include<std_msgs/Float64.h>
#include "qcgenerator.h"
#include<termios.h>
#include<gazebo_msgs/LinkStates.h>
#include<sensor_msgs/JointState.h>
#include"pidcontroller.h"

using namespace  std;
using namespace  Eigen;

double saturate(double a, double min, double max){
    if(a<min){return min;ROS_INFO("subceeding!");}
    else if(a>max){return max;ROS_INFO("exceeding!");}
    else{return a;}
}

ros::Publisher pub1; ros::Publisher pub2; ros::Publisher pub3; ros::Publisher pub4;
ros::Publisher pub5; ros::Publisher pub6; ros::Publisher pub7; ros::Publisher pub8;
ros::Publisher pub9; ros::Publisher pub10; ros::Publisher pub11; ros::Publisher pub12;
ros::Publisher pub13; ros::Publisher pub14; ros::Publisher pub15; ros::Publisher pub16;
ros::Publisher pub17; ros::Publisher pub18; ros::Publisher pub19; ros::Publisher pub20;
ros::Publisher pub21; ros::Publisher pub22; ros::Publisher pub23; ros::Publisher pub24;
ros::Publisher pub25; ros::Publisher pub26; ros::Publisher pub27; ros::Publisher pub28;
ros::Publisher pub29; ros::Publisher pub30; ros::Publisher pub31;

void numplot(double num,double min,double max){
  //⬛

  QString str;
  int l=100;
  int n=int((num-min)/(max-min)*l);
  if (num<min){n=0;}
  if (num>max){n=100;}
  str+=QString::number(min);
  str+="|";
  if (n<=l/2){
      for (int i = 0; i < n; ++i) {
          str+=" ";
      }
      for (int i = 0; i < l/2-n; ++i) {
          str+="|";

      }
      str+="|";
      for (int i = 0; i < l/2; ++i) {
          str+=" ";
      }
  }
  else {
      for (int i = 0; i < l/2; ++i) {
          str+=" ";
      }
      for (int i = 0; i < n-l/2; ++i) {
          str+="|";

      }
      str+="|";
      for (int i = 0; i < l-n; ++i) {
          str+=" ";
      }

  }

  str+="|";
  str+=QString::number(max);
  str+="=>";str+=QString::number(num);
  qDebug()<<str;
qDebug()<<"";

}

double move_rest_back(double max,double t_local,double T_start ,double T_move,double T_rest,double T_back){
    double c3=(10*max)/pow(T_move,3);
    double c4=-(15*max)/pow(T_move,4);
    double c5=(6*max)/pow(T_move,5);
    double c3_r=(10*max)/pow(T_back,3);
    double c4_r=-(15*max)/pow(T_back,4);
    double c5_r=(6*max)/pow(T_back,5);
    double T_end=T_start+T_move+T_rest+T_back;
    double theta=0;
    if(t_local<T_start){theta=0;}
    else if (t_local<T_start+T_move){theta=c3*pow(t_local-T_start,3)+c4*pow(t_local-T_start,4)+c5*pow(t_local-T_start,5);}
    else if (t_local<T_start+T_move+T_rest){theta=max;}
    else if (t_local<T_start+T_move+T_rest+T_back){theta=c3_r*pow(T_end-t_local,3)+c4_r*pow(T_end-t_local,4)+c5_r*pow(T_end-t_local,5);}
    return theta;
}

void  SendGazebo(QList<LinkM> links,MatrixXd RollModifieds, double PitchModifieds, double theta_r, double phi_r, double theta_l, double phi_l){
    if(links.count()<28){qDebug()<<"index err";return;}
    std_msgs::Float64 data;

    data.data=links[1].JointAngle;    pub1.publish(data);
    data.data=links[2].JointAngle+RollModifieds(0,0);    pub2.publish(data);
    data.data=links[3].JointAngle+PitchModifieds;    pub3.publish(data);
    data.data=links[4].JointAngle;    pub4.publish(data);
    // data.data=links[5].JointAngle+theta_r;    pub5.publish(data);
    data.data=saturate(links[5].JointAngle,-M_PI/5.4,M_PI/4)+theta_r;    pub5.publish(data);
    data.data=links[6].JointAngle+phi_r;    pub6.publish(data);
    data.data=links[7].JointAngle;    pub7.publish(data);
    data.data=links[8].JointAngle+RollModifieds(1,0);    pub8.publish(data);
    data.data=links[9].JointAngle+PitchModifieds;    pub9.publish(data);
    data.data=links[10].JointAngle;    pub10.publish(data);
    //data.data=links[11].JointAngle+theta_l;    pub11.publish(data);
    data.data=saturate(links[11].JointAngle,-M_PI/5.4,M_PI/4)+theta_l;    pub11.publish(data);
    data.data=links[12].JointAngle+phi_l;    pub12.publish(data);
    data.data=links[13].JointAngle;    pub13.publish(data);
    data.data=links[14].JointAngle;    pub14.publish(data);
    data.data=links[15].JointAngle;    pub15.publish(data);
    data.data=links[16].JointAngle;    pub16.publish(data);
    data.data=links[17].JointAngle;    pub17.publish(data);
    data.data=links[18].JointAngle;    pub18.publish(data);
    data.data=links[19].JointAngle;    pub19.publish(data);
    data.data=links[20].JointAngle;    pub20.publish(data);
    data.data=links[21].JointAngle;    pub21.publish(data);
    data.data=links[22].JointAngle;    pub22.publish(data);
    data.data=links[23].JointAngle;    pub23.publish(data);
    data.data=links[24].JointAngle;    pub24.publish(data);
    data.data=links[25].JointAngle;    pub25.publish(data);
    data.data=links[26].JointAngle;    pub26.publish(data);
    data.data=links[27].JointAngle;    pub27.publish(data);
    data.data=links[28].JointAngle;    pub28.publish(data);
    data.data=0;    pub29.publish(data);
    data.data=0;    pub30.publish(data);
    data.data=0;    pub31.publish(data);
}

int getch()
{
    static struct termios oldt, newt;
    tcgetattr( STDIN_FILENO, &oldt);           // save old settings
    newt = oldt;
    newt.c_lflag &= ~(ICANON);                 // disable buffering
    tcsetattr( STDIN_FILENO, TCSANOW, &newt);  // apply new settings

    int c = getchar();  // read character (non-blocking)

    tcsetattr( STDIN_FILENO, TCSANOW, &oldt);  // restore old settings
    return c;
}

MinimumJerkInterpolation CoefGen;

//data of left foot sensor
int a,b,c,d,e,f,g,h;

//angles of ankle adaptation
double teta_motor_L,teta_motor_R;//pitch
double phi_motor_L,phi_motor_R;//roll
double PitchModified;

int qc_offset[12];
bool qc_initial_bool;

//
int bump_pushed[8];
int bump_notpushed[8];
bool bump_initialize;
float InputFreq=0;
float freq=1;
void receiveFreq(const std_msgs::Float64 msg)
{
InputFreq=msg.data;
   //qDebug()<<"InputFreq="<<InputFreq;
}
void receiveFootSensor(const std_msgs::Int32MultiArray& msg)
{
    if (msg.data.size()!=8) {
        qDebug("the size of sensor data is in wrong");
        return;
    }
    if(bump_initialize){
        for (int i = 0; i < 8; ++i) {
            bump_pushed[i]=msg.data[i];
        }
        bump_initialize=false;
    }
    //ROS_INFO("I heard: [%d  %d %d %d %d  %d %d %d]", (int)msg.data[0],(int)msg.data[1],(int)msg.data[2],(int)msg.data[3],(int)msg.data[4],(int)msg.data[5],(int)msg.data[6],(int)msg.data[7]);
    int temp[8];

    bump_notpushed[0]=1012;bump_notpushed[1]= 924;bump_notpushed[2]=3038;bump_notpushed[3]=3098;
    bump_notpushed[4]=3042;bump_notpushed[5]=3008;bump_notpushed[6]=1133;bump_notpushed[7]=1016;
    temp[0]=msg.data[0]-bump_notpushed[0];
    temp[1]=-1*(msg.data[1]-bump_notpushed[1]);
    temp[2]=msg.data[2]-bump_notpushed[2];
    temp[3]=-1*(msg.data[3]-bump_notpushed[3]);

    //normalizing data of sensors
    a=temp[0]*(100.0/(bump_pushed[0]-bump_notpushed[0]));
    b=temp[1]*(100.0/(bump_notpushed[1]-bump_pushed[1]));
    c=temp[2]*(100.0/(bump_pushed[2]-bump_notpushed[2]));
    d=temp[3]*(100.0/(bump_notpushed[3]-bump_pushed[3]));

    //ROS_INFO("I heard a b c d: [%d  %d %d %d]", a,b,c,d);

    temp[4]=msg.data[4]-bump_notpushed[4];
    temp[5]=-1*(msg.data[5]-bump_notpushed[5]);
    temp[6]=msg.data[6]-bump_notpushed[6];
    temp[7]=-1*(msg.data[7]-bump_notpushed[7]);

    //normalizing data of sensors
    e=temp[4]*(100.0/(bump_pushed[4]-bump_notpushed[4]));
    f=temp[5]*(100.0/(bump_notpushed[5]-bump_pushed[5]));
    g=temp[6]*(100.0/(bump_pushed[6]-bump_notpushed[6]));
    h=temp[7]*(100.0/(bump_notpushed[7]-bump_pushed[7]));

    // ROS_INFO("I heard e f g h: [%d  %d %d %d]", e,f,g,h);

    //deleting data with negative sign
    if (a<0){a=0;} if (b<0){b=0;} if (c<0){c=0;} if (d<0){d=0;}
    if (e<0){e=0;} if (f<0){f=0;} if (g<0){g=0;} if (h<0){h=0;}
}

void qc_initial(const sensor_msgs::JointState & msg){
    if (qc_initial_bool){

        for (int i = 0; i < 12; ++i) {
            qc_offset[i]=int(msg.position[i+1]);

        }

        qc_initial_bool=false;

        ROS_INFO("Offset=%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t%d\t\nInitialized!",
                 qc_offset[0],qc_offset[1],qc_offset[2],qc_offset[3],qc_offset[4],
                qc_offset[5],qc_offset[6],qc_offset[7],qc_offset[8],qc_offset[9],
                qc_offset[10],qc_offset[11]);}
}

double Fzl,Fzr,Mxl,Mxr;
void FT_left_feedback(const geometry_msgs::Wrench &msg){
    Fzl=msg.force.z;
    Mxl=msg.torque.y;
}

void FT_right_feedback(const geometry_msgs::Wrench &msg){
    Fzr=msg.force.z;
    Mxr=msg.torque.x;
}
bool wrench_init_bool;
void WrenchHomming(){
    double threshold= .5;
    double k=10;
    double max=20;
    if(wrench_init_bool && ((abs(Mxl)>threshold) || (abs(Mxr)>threshold))){
        ROS_INFO_ONCE("wrench initializing!");
        if (abs(Mxl)>threshold){
            if(k*Mxl<-max){qc_offset[5]-=max;}
            else if(k*Mxl>max){qc_offset[5]+=max;}
            else {qc_offset[5]+=k*Mxl;}
        }
        if (abs(Mxr)>threshold){
            if(k*Mxr<-max){qc_offset[0]-=max;}
            else if(k*Mxr>max){qc_offset[0]+=max;}
            else {qc_offset[0]+=k*Mxr;}
        }

    }
    if((abs(Mxl)<threshold) && (abs(Mxr)<threshold)){
        wrench_init_bool=false;
        ROS_INFO_ONCE("ankles initilalzed!");
    }
}

MatrixXd quater2rot(double w,double x,double y, double z){
    MatrixXd R(3,3);
    R<<w*w+x*x-y*y-z*z,2*x*y-2*w*z,2*x*z+2*w*y,
            2*x*y+2*w*z,w*w-x*x+y*y-z*z,2*y*z-2*w*x,
            2*x*z-2*w*y,2*y*z+2*w*x,w*w-x*x-y*y+z*z;
    return R;
}

void ankle_states(const gazebo_msgs::LinkStates& msg){
    double x_left, x_right,y_left, y_right,z_left, z_right;
    Vector3d vec_A_E;
    Vector3d vec_B_F;
    Vector3d vec_C_G;
    Vector3d vec_D_H;

    vec_A_E<<-.085,
            -.08,
            -.11;
    vec_B_F<<.135,
            -.08,
            -.11;
    vec_C_G<<.135,
            .08,
            -.11;
    vec_D_H<<-.085,
            .08,
            -.11;

    x_left=msg.pose[7].position.x;
    x_right=msg.pose[13].position.x;
    y_left=msg.pose[7].position.y;
    y_right=msg.pose[13].position.y;
    z_left=msg.pose[7].position.z;
    z_right=msg.pose[13].position.z;

    MatrixXd R_left(3,3);
    MatrixXd R_right(3,3);
    R_left=quater2rot(msg.pose[7].orientation.w,msg.pose[7].orientation.x,msg.pose[7].orientation.y,msg.pose[7].orientation.z);
    R_right=quater2rot(msg.pose[13].orientation.w,msg.pose[13].orientation.x,msg.pose[13].orientation.y,msg.pose[13].orientation.z);
    Vector3d temp;
    temp=R_left*vec_A_E;    //A=temp(2)+z_left;
    a=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
    temp=R_left*vec_B_F;    //B=temp(2)+z_left;
    b=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
    temp=R_left*vec_C_G;    //C=temp(2)+z_left;
    c=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
    temp=R_left*vec_D_H;    //D=temp(2)+z_left;
    d=int(((.02-temp(2)-z_left)+abs(.02-temp(2)-z_left))*5000/2);
    temp=R_right*vec_A_E;    //E=temp(2)+z_right;
    e=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
    temp=R_right*vec_B_F;    //F=temp(2)+z_right;
    f=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
    temp=R_right*vec_C_G;    //G=temp(2)+z_right;
    g=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
    temp=R_right*vec_D_H;    //H=temp(2)+z_right;
    h=int(((.02-temp(2)-z_right)+abs(.02-temp(2)-z_right))*5000/2);
    //ROS_INFO("z_left=%f  A=%d,B=%d,C=%d,D=%d\tz_right=%f  E=%d,F=%d,G=%d,H=%d",z_left,a,b,c,d,z_right,e,f,g,h);
    //    ROS_INFO("x_left=%f,x_right=%f,y_left=%f,y_right=%fz_left=%f,z_right=%f",x_left,x_right,y_left,y_right,z_left,z_right);
}

bool simulation;
Robot SURENA;//model of robot & kinematics funcs(IK & FK)
Robot SURENA_turning;
TaskSpaceOnline3 OnlineTaskSpace;
QList<LinkM> links;
MatrixXd PoseRoot;//position of pelvis respected to global coordinate
MatrixXd PoseRFoot;//position of right ankle joint respected to global coordinate
MatrixXd PoseLFoot;//position of left ankle joint respected to global coordinate
double GlobalTime;
double  DurationOfStartPhase;
double  DurationOfendPhase;

void StartPhase(){
    if ( GlobalTime<=DurationOfStartPhase) {

        MinimumJerkInterpolation Coef;
        MatrixXd ZPosition(1,2);
        ZPosition<<OnlineTaskSpace.InitialPelvisHeight,OnlineTaskSpace.ReferencePelvisHeight;
        MatrixXd ZVelocity(1,2);
        ZVelocity<<0.000,0.000;
        MatrixXd ZAcceleration(1,2);
        ZAcceleration<<0.000,0.000;
        MatrixXd Time(1,2);
        Time<<0,DurationOfStartPhase;
        MatrixXd CoefZStart =Coef.Coefficient(Time,ZPosition,ZVelocity,ZAcceleration);
        double zStart=0;
        double yStart=0;
        double xStart=0;
        GlobalTime=GlobalTime+OnlineTaskSpace._timeStep;

        MatrixXd outputZStart= Coef.GetAccVelPos(CoefZStart,GlobalTime,0,5);
        zStart=outputZStart(0,0);

        PoseRoot<<xStart,yStart,zStart,0,0,0;
        PoseRFoot<<0,
                -0.11500,
                0.112000,
                0,
                0,
                0;

        PoseLFoot<<0,
                0.11500,
                0.11200,
                0,
                0,
                0;

        SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
        SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);

        SURENA_turning.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
        SURENA_turning.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);

        MinimumJerkInterpolation CoefOffline;
        double D_pitch=-1*OnlineTaskSpace.HipPitchModification*(M_PI/180);
        double TstartofPitchModify=DurationOfStartPhase/6;
        double TendofPitchModify=DurationOfStartPhase;
        double D_time=TendofPitchModify-TstartofPitchModify;
        if (GlobalTime>=TstartofPitchModify && GlobalTime<=TendofPitchModify){
            MatrixXd Ct_pitch_st(1,2);
            Ct_pitch_st<<0 ,D_time;
            MatrixXd Cp_pitch_st(1,2);
            Cp_pitch_st<<0, D_pitch;
            MatrixXd Cv_pitch_st(1,2);
            Cv_pitch_st<<0 ,0;
            MatrixXd Ca_pitch_st(1,2);
            Ca_pitch_st<<0 ,0;
            MatrixXd C_pitch_st=CoefOffline.Coefficient(Ct_pitch_st,Cp_pitch_st,Cv_pitch_st,Ca_pitch_st);

            MatrixXd output=Coef.GetAccVelPos(C_pitch_st,GlobalTime-(TstartofPitchModify),0,5);
            PitchModified=output(0,0);

        }

    }
}

void EndPhase(){

    if (GlobalTime>=(DurationOfStartPhase+OnlineTaskSpace.MotionTime) && GlobalTime<=DurationOfendPhase+DurationOfStartPhase+OnlineTaskSpace.MotionTime) {

        MinimumJerkInterpolation Coef;
        MatrixXd ZPosition(1,2);
        //ZPosition<<Pz(0,0),Pz(0,0)-OnlineTaskSpace.ReferencePelvisHeight+OnlineTaskSpace.InitialPelvisHeight;//this one should be edited
        ZPosition<<OnlineTaskSpace.ReferencePelvisHeight,OnlineTaskSpace.InitialPelvisHeight;//this one should be edited
        MatrixXd ZVelocity(1,2);
        ZVelocity<<0.000,0.000;
        MatrixXd ZAcceleration(1,2);
        ZAcceleration<<0.000,0.000;

        MatrixXd Time(1,2);
        Time<<DurationOfStartPhase+OnlineTaskSpace.MotionTime,DurationOfStartPhase+OnlineTaskSpace.MotionTime+DurationOfendPhase;
        MatrixXd CoefZStart =Coef.Coefficient(Time,ZPosition,ZVelocity,ZAcceleration);

        double zStart=0;
        double yStart=0;
        double xStart=0;
        GlobalTime=GlobalTime+OnlineTaskSpace._timeStep;

        MatrixXd outputZStart= Coef.GetAccVelPos(CoefZStart,GlobalTime,DurationOfStartPhase+OnlineTaskSpace.MotionTime,5);
        zStart=outputZStart(0,0);

        PoseRoot<<xStart,yStart,zStart,0,0,0;

        PoseRFoot<<0,
                -0.11500,
                OnlineTaskSpace.currentRightFootZ,
                0,
                0,
                0;

        PoseLFoot<<0,
                0.11500,
                OnlineTaskSpace.currentLeftFootZ,
                0,
                0,
                0;

        SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,"Body", PoseRoot);
        SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,"Body", PoseRoot);

        links = SURENA.GetLinks();

        MinimumJerkInterpolation CoefOffline;
        double D_pitch=-1*OnlineTaskSpace.HipPitchModification*(M_PI/180);
        double TstartofPitchModify=DurationOfendPhase/6+DurationOfStartPhase+OnlineTaskSpace.MotionTime;
        double TendofPitchModify=DurationOfendPhase*5/6+DurationOfStartPhase+OnlineTaskSpace.MotionTime;
        double D_time=TendofPitchModify-TstartofPitchModify;
        if (GlobalTime>=TstartofPitchModify && GlobalTime<=TendofPitchModify){
            MatrixXd Ct_pitch_st(1,2);
            Ct_pitch_st<<-D_time, 0;
            MatrixXd Cp_pitch_st(1,2);
            Cp_pitch_st<<D_pitch, 0;
            MatrixXd Cv_pitch_st(1,2);
            Cv_pitch_st<<0 ,0;
            MatrixXd Ca_pitch_st(1,2);
            Ca_pitch_st<<0 ,0;
            MatrixXd C_pitch_st=CoefOffline.Coefficient(Ct_pitch_st,Cp_pitch_st,Cv_pitch_st,Ca_pitch_st);

            MatrixXd output=Coef.GetAccVelPos(C_pitch_st,GlobalTime-(TendofPitchModify),-D_time,5);
            PitchModified=output(0,0);

        }

    }
}

void hand_move(){
        double t_h=GlobalTime-DurationOfStartPhase-OnlineTaskSpace.TStart;
        while(t_h>2*(OnlineTaskSpace.TDs+OnlineTaskSpace.TSS)){
         t_h-= 2*(OnlineTaskSpace.TDs+OnlineTaskSpace.TSS);
        }

        double l_h=0;
        double r_h=0;

        if (t_h>OnlineTaskSpace.TDs && t_h<OnlineTaskSpace.TDs+OnlineTaskSpace.TSS){
            l_h=sin((t_h-OnlineTaskSpace.TDs)*M_PI/OnlineTaskSpace.TSS);
            l_h=-M_PI/6*l_h*l_h;
        }
        if (t_h>2*OnlineTaskSpace.TDs+OnlineTaskSpace.TSS){
            r_h=sin((t_h-2*OnlineTaskSpace.TDs+OnlineTaskSpace.TSS)*M_PI/OnlineTaskSpace.TSS);
            r_h=-M_PI/6*r_h*r_h;
        }
//        if (t_h<OnlineTaskSpace.TDs+OnlineTaskSpace.TSS){
//            l_h=sin((t_h)*M_PI/(OnlineTaskSpace.TDs+OnlineTaskSpace.TSS));
//            l_h=-M_PI/10*l_h*l_h;
//        }
//        if (t_h>OnlineTaskSpace.TDs+OnlineTaskSpace.TSS){
//            r_h=sin((t_h-(OnlineTaskSpace.TDs+OnlineTaskSpace.TSS))*M_PI/(OnlineTaskSpace.TDs+OnlineTaskSpace.TSS));
//            r_h=-M_PI/10*r_h*r_h;

//        }
//        if (t_h<OnlineTaskSpace.TDs+OnlineTaskSpace.TSS){
//            l_h=sin((t_h)*M_PI/(OnlineTaskSpace.TDs+OnlineTaskSpace.TSS));
//            l_h=-M_PI*l_h*l_h*exp(t_h)/exp(OnlineTaskSpace.TDs+OnlineTaskSpace.TSS);
//        }
//        if (t_h>OnlineTaskSpace.TDs+OnlineTaskSpace.TSS){
//            r_h=sin((t_h-(OnlineTaskSpace.TDs+OnlineTaskSpace.TSS))*M_PI/(OnlineTaskSpace.TDs+OnlineTaskSpace.TSS));
//            r_h=-M_PI*r_h*r_h*exp(t_h-(OnlineTaskSpace.TDs+OnlineTaskSpace.TSS))/exp(OnlineTaskSpace.TDs+OnlineTaskSpace.TSS);

//        }

        if(GlobalTime<=DurationOfStartPhase+OnlineTaskSpace.TStart){l_h=0;r_h=0;}
        if(GlobalTime>=DurationOfStartPhase+OnlineTaskSpace.MotionTime){l_h=0;r_h=0;}

       links[15].JointAngle=r_h;
       links[22].JointAngle=l_h;
}

double ankle_discharge_timer_l;
double ankle_discharge_timer_r;
void ankleAdaptation(){
    //parameters of ankle adaptation
    double k1,k2,k3,k4;
    //k1=0.000015;    k2=0.000015;    k3=0.000015;    k4=0.000015;
    k1=0.0000215;
    k2=0.0000215;
    k3=0.0000215;
    k4=0.0000215;

double threshold=4;
double discharge_duration=1;
    if (a>threshold ||b>threshold ||c>threshold ||d>threshold){//left
        //-----------------Pitch left ankle motor control---------------//
        if (abs(b-a)>=abs(c-d)) {
            if (abs(a-b)<100) {
                teta_motor_L=1*teta_motor_L+k1*(a-b);
            }
        }
        else {
            if (abs(d-c)<100) {
                teta_motor_L=1*teta_motor_L+k1*(d-c);
            }

        }

        //----------------Roll left ankle motor control---------------//
        if (abs(c-b)>=abs(d-a)) {
            if (abs(c-b)<100) {
                phi_motor_L=1*phi_motor_L+k2*(c-b);
            }
        }
        else {
            if (abs(a-d)<100) {
                phi_motor_L=1*phi_motor_L+k2*(d-a);
            }
        }

    ankle_discharge_timer_l=0;
    }
else{
        ankle_discharge_timer_l+=OnlineTaskSpace._timeStep;

        phi_motor_L=phi_motor_L*pow(cos(2/M_PI*ankle_discharge_timer_l),2)/pow(cos(2/M_PI*(ankle_discharge_timer_l-OnlineTaskSpace._timeStep)),2);
        teta_motor_L=teta_motor_L*pow(cos(2/M_PI*ankle_discharge_timer_l),2)/pow(cos(2/M_PI*(ankle_discharge_timer_l-OnlineTaskSpace._timeStep)),2);
   if(ankle_discharge_timer_l>discharge_duration){
       ankle_discharge_timer_l=discharge_duration;
       phi_motor_L=0;
       teta_motor_L=0;
   }
    }
        if(e>threshold ||f>threshold ||g>threshold ||h>threshold){//right

        //-----------------Pitch left ankle motor control---------------//
        if (abs(f-e)>=abs(g-h)) {
            if (abs(e-f)<100) {
                teta_motor_R=1*teta_motor_R+k3*(e-f);
            }
        }
        else {
            if (abs(h-g)<100) {
                teta_motor_R=1*teta_motor_R+k3*(h-g);
            }
        }

        //----------------Roll left ankle motor control---------------//
        if (abs(g-f)>=abs(h-e)) {
            if (abs(g-f)<100) {
                phi_motor_R=1*phi_motor_R+k4*(g-f);
            }
        }
        else {
            if (abs(e-h)<100) {
                phi_motor_R=1*phi_motor_R+k4*(h-e);
            }
        }

    //------------------------saturation of ankle motors----------------------------//
    if ((abs(phi_motor_L))>0.9) {phi_motor_L=0.9;}
    if ((abs(teta_motor_L))>0.9) {teta_motor_L=0.9;}
    if ((abs(phi_motor_R))>0.9) {phi_motor_L=0.9;}
    if ((abs(teta_motor_R))>0.9) {teta_motor_R=0.9;}
ankle_discharge_timer_r=0;
 }

else{
        ankle_discharge_timer_r+=OnlineTaskSpace._timeStep;

        phi_motor_R=phi_motor_R*pow(cos(2/M_PI*ankle_discharge_timer_r),2)/pow(cos(2/M_PI*(ankle_discharge_timer_r-OnlineTaskSpace._timeStep)),2);
        teta_motor_R=teta_motor_R*pow(cos(2/M_PI*ankle_discharge_timer_r),2)/pow(cos(2/M_PI*(ankle_discharge_timer_r-OnlineTaskSpace._timeStep)),2);
        if(ankle_discharge_timer_r>discharge_duration){
            ankle_discharge_timer_r=discharge_duration;
            phi_motor_R=0;
            teta_motor_R=0;
        }
   }
}
MatrixXd Coef_teta_motor_L; MatrixXd Coef_phi_motor_L;
MatrixXd Coef_teta_motor_R; MatrixXd Coef_phi_motor_R;

//void ankleAdaptationDischargeCoef(){
//    MatrixXd Time(1,2);MatrixXd Position(1,2);MatrixXd Velocity(1,2);MatrixXd Acceleration(1,2);
//    Time<<0,DurationOfStartPhase;
//    Velocity<<0.000,0.000;
//    Acceleration<<0.000,0.000;
//    Position<<teta_motor_L,0;
//    Coef_teta_motor_L =CoefGen.Coefficient(Time,Position,Velocity,Acceleration);
//    Position<<teta_motor_R,0;
//    Coef_teta_motor_R =CoefGen.Coefficient(Time,Position,Velocity,Acceleration);
//    Position<<phi_motor_L,0;
//    Coef_phi_motor_L =CoefGen.Coefficient(Time,Position,Velocity,Acceleration);
//    Position<<phi_motor_L,0;
//    Coef_phi_motor_R =CoefGen.Coefficient(Time,Position,Velocity,Acceleration);
//}

//void ankleAdaptationDischarge(){
//    teta_motor_L= CoefGen.GetAccVelPos(Coef_teta_motor_L,GlobalTime,0,5)(0,0);
//    teta_motor_R= CoefGen.GetAccVelPos(Coef_teta_motor_R,GlobalTime,0,5)(0,0);
//    phi_motor_L= CoefGen.GetAccVelPos(Coef_phi_motor_L,GlobalTime,0,5)(0,0);
//    phi_motor_R= CoefGen.GetAccVelPos(Coef_phi_motor_R,GlobalTime,0,5)(0,0);
//}

int main(int argc, char **argv)
{
OnlineTaskSpace.NStride=10;
OnlineTaskSpace.NStep=OnlineTaskSpace.NStride*2;

    OnlineTaskSpace.YpMax=.08;
    OnlineTaskSpace.Yd=OnlineTaskSpace.YpMax;
    OnlineTaskSpace.YStMax=OnlineTaskSpace.YpMax;
    OnlineTaskSpace.YEndMax=OnlineTaskSpace.YpMax;
    OnlineTaskSpace.TStart=2;
    OnlineTaskSpace.TDs=1;
    OnlineTaskSpace.TSS=.1;
    OnlineTaskSpace.AnkleMaximumHeight=0.001;
    OnlineTaskSpace.ReferencePelvisHeight=.89+.02;
    OnlineTaskSpace.Tc=OnlineTaskSpace.TDs+OnlineTaskSpace.TSS;
    OnlineTaskSpace.Tm2=0.35*OnlineTaskSpace.TSS;
    OnlineTaskSpace.TGait=OnlineTaskSpace.TStart+OnlineTaskSpace.NStride*2*OnlineTaskSpace.Tc;
    OnlineTaskSpace.MotionTime=OnlineTaskSpace.TStart+OnlineTaskSpace.NStride*2*OnlineTaskSpace.Tc+OnlineTaskSpace.TDs+OnlineTaskSpace.TEnd;
    OnlineTaskSpace.TMinPelvisY=0.5*OnlineTaskSpace.TDs; // The time that pelvis reaches its minimum distance in y direction
    OnlineTaskSpace.TMaxAnkle=OnlineTaskSpace.TDs+0.35*OnlineTaskSpace.TSS;//0.53 % The time that ankle reaches its maximum distance in z direction
    OnlineTaskSpace.TMaxPelvisY=OnlineTaskSpace.TDs+0.5*OnlineTaskSpace.TSS; // The time that pelvis reaches its maximum distance in y direction
    OnlineTaskSpace.T_end_of_first_SS=0.01;
    OnlineTaskSpace.T_end_of_SS=0.01;//TSS*.3;
    OnlineTaskSpace.T_end_of_last_SS=0.01;
    OnlineTaskSpace.h_end_of_SS=0.001;

    OnlineTaskSpace.T_end_p_sx_rel=OnlineTaskSpace.TDs+0.25*OnlineTaskSpace.TEnd;
    OnlineTaskSpace.T_end_p_sx=OnlineTaskSpace.TGait+OnlineTaskSpace.TDs+0.25*OnlineTaskSpace.TEnd;
    OnlineTaskSpace.T_end_p_sy=OnlineTaskSpace.TGait+OnlineTaskSpace.TDs+0.2*OnlineTaskSpace.TEnd;
    OnlineTaskSpace.T_end_p_dy=OnlineTaskSpace.TGait+OnlineTaskSpace.TDs+0.4*OnlineTaskSpace.TEnd;
    OnlineTaskSpace.T_end_p_ey=OnlineTaskSpace.TGait+OnlineTaskSpace.TDs+0.8*OnlineTaskSpace.TEnd;
    OnlineTaskSpace.T_end_a_s=OnlineTaskSpace.TGait+OnlineTaskSpace.TDs;
    OnlineTaskSpace.T_end_a_e=OnlineTaskSpace.TGait+OnlineTaskSpace.TDs+0.5*OnlineTaskSpace.TEnd;
    OnlineTaskSpace.T_end_a_d=OnlineTaskSpace.TGait+OnlineTaskSpace.TDs+.6*(OnlineTaskSpace.T_end_a_e-OnlineTaskSpace.T_end_a_s);0.45;
    OnlineTaskSpace.T_st_p_sy=0.2*OnlineTaskSpace.TStart;
    OnlineTaskSpace.T_st_p_dy=0.5*OnlineTaskSpace.TStart;//0.65*TStart;
    OnlineTaskSpace.T_st_p_ey=0.8*OnlineTaskSpace.TStart;
    OnlineTaskSpace.T_st_p_sx=0.7*OnlineTaskSpace.TStart;
    OnlineTaskSpace.T_s_st=.5*OnlineTaskSpace.TStart;
    OnlineTaskSpace.T_st_a_d=OnlineTaskSpace.T_s_st+0.45*(OnlineTaskSpace.TStart-OnlineTaskSpace.T_s_st);

    OnlineTaskSpace.CoeffArrayAnkle();

    OnlineTaskSpace.CoeffArrayPelvis();

    simulation=false;
    qc_initial_bool=!simulation;
    bump_initialize=false;
    wrench_init_bool=false;

    vector<double> cntrl(13);
    QCgenerator QC;
    for (int i = 0; i < 12; ++i) {
        qc_offset[i]=0;
    }

    teta_motor_L=0;
    teta_motor_R=0;
    phi_motor_L=0;
    phi_motor_R=0;

    double footSensorSaturation=75;//if all sensors data are bigger than this amount, this means the foot is landed on the ground
    double footSensorthreshold=4;// will start orientaition correction

    GlobalTime=0;
    DurationOfStartPhase=2;
    DurationOfendPhase=6;

    MatrixXd RollModified(2,1);RollModified<<0,0;//parameters for hip roll angles charge, for keep pelvis straight
    PitchModified=0;
    PoseRoot.resize(6,1); //pelvis trajectory from taskspace_online,xyzrpy
    PoseRFoot.resize(6,1);//right ankle joint trajectory from taskspace_online,xyzrpy
    PoseLFoot.resize(6,1);//left ankle joint trajectory from taskspace_online,xyzrpy

    //*******************This part of code is for initialization of joints of the robot for walking**********************************
    int count = 0;

    ros::init(argc, argv, "myNode");
    ros::NodeHandle nh;
    ros::Publisher  chatter_pub  = nh.advertise<std_msgs::Int32MultiArray>("jointdata/qc",1000);
    ros::Publisher  contact_flag  = nh.advertise<std_msgs::Int32MultiArray>("contact_flag_timing",100);
    ros::Subscriber sub = nh.subscribe("/surena/bump_sensor_state", 1000, receiveFootSensor);
    ros::Subscriber frequncysub = nh.subscribe("/freq", 1000, receiveFreq);
    ros::Subscriber ft_left = nh.subscribe("/surena/ft_l_state",1000,FT_left_feedback);
    ros::Subscriber ft_right = nh.subscribe("/surena/ft_r_state",1000,FT_right_feedback);
    ros::Subscriber qcinit = nh.subscribe("/surena/inc_joint_state", 1000, qc_initial);
    if(simulation){//gazebo publishers
        pub1  = nh.advertise<std_msgs::Float64>("rrbot/joint1_position_controller/command",1000);
        pub2  = nh.advertise<std_msgs::Float64>("rrbot/joint2_position_controller/command",1000);
        pub3  = nh.advertise<std_msgs::Float64>("rrbot/joint3_position_controller/command",1000);
        pub4  = nh.advertise<std_msgs::Float64>("rrbot/joint4_position_controller/command",1000);
        pub5  = nh.advertise<std_msgs::Float64>("rrbot/joint5_position_controller/command",1000);
        pub6  = nh.advertise<std_msgs::Float64>("rrbot/joint6_position_controller/command",1000);
        pub7  = nh.advertise<std_msgs::Float64>("rrbot/joint7_position_controller/command",1000);
        pub8  = nh.advertise<std_msgs::Float64>("rrbot/joint8_position_controller/command",1000);
        pub9  = nh.advertise<std_msgs::Float64>("rrbot/joint9_position_controller/command",1000);
        pub10 = nh.advertise<std_msgs::Float64>("rrbot/joint10_position_controller/command",1000);
        pub11 = nh.advertise<std_msgs::Float64>("rrbot/joint11_position_controller/command",1000);
        pub12 = nh.advertise<std_msgs::Float64>("rrbot/joint12_position_controller/command",1000);
        pub13 = nh.advertise<std_msgs::Float64>("rrbot/joint13_position_controller/command",1000);
        pub14 = nh.advertise<std_msgs::Float64>("rrbot/joint14_position_controller/command",1000);
        pub15 = nh.advertise<std_msgs::Float64>("rrbot/joint15_position_controller/command",1000);
        pub16 = nh.advertise<std_msgs::Float64>("rrbot/joint16_position_controller/command",1000);
        pub17 = nh.advertise<std_msgs::Float64>("rrbot/joint17_position_controller/command",1000);
        pub18 = nh.advertise<std_msgs::Float64>("rrbot/joint18_position_controller/command",1000);
        pub19 = nh.advertise<std_msgs::Float64>("rrbot/joint19_position_controller/command",1000);
        pub20 = nh.advertise<std_msgs::Float64>("rrbot/joint20_position_controller/command",1000);
        pub21 = nh.advertise<std_msgs::Float64>("rrbot/joint21_position_controller/command",1000);
        pub22 = nh.advertise<std_msgs::Float64>("rrbot/joint22_position_controller/command",1000);
        pub23 = nh.advertise<std_msgs::Float64>("rrbot/joint23_position_controller/command",1000);
        pub24 = nh.advertise<std_msgs::Float64>("rrbot/joint24_position_controller/command",1000);
        pub25 = nh.advertise<std_msgs::Float64>("rrbot/joint25_position_controller/command",1000);
        pub26 = nh.advertise<std_msgs::Float64>("rrbot/joint26_position_controller/command",1000);
        pub27 = nh.advertise<std_msgs::Float64>("rrbot/joint27_position_controller/command",1000);
        pub28 = nh.advertise<std_msgs::Float64>("rrbot/joint28_position_controller/command",1000);
        pub29 = nh.advertise<std_msgs::Float64>("rrbot/joint29_position_controller/command",1000);
        pub30 = nh.advertise<std_msgs::Float64>("rrbot/joint30_position_controller/command",1000);
        pub31 = nh.advertise<std_msgs::Float64>("rrbot/joint31_position_controller/command",1000);
    }
    ros::Subscriber ankleStates = nh.subscribe("/gazebo/link_states", 10, ankle_states);

    int32_t contact_flag_timing=1000;    int32_t contact_flag_sensor=1000;    int32_t contact_flag_sensor2=1000;

    ros::Rate loop_rate(200);
    std_msgs::Int32MultiArray msg;
    std_msgs::Int32MultiArray msg_contact_flag;
    std_msgs::MultiArrayDimension msg_dim;

    msg_dim.label = "joint_position";
    msg_dim.size = 1;
    msg.layout.dim.clear();
    msg.layout.dim.push_back(msg_dim);
    msg_contact_flag.data.clear();
    msg_contact_flag.data.push_back(contact_flag_timing);

    links=SURENA.GetLinks();
    OnlineTaskSpace.StepNumber=1;

    MatrixXd P;

    ROS_INFO("press any key to start!");
    getch();
    ROS_INFO("started!");

    bool firstcontactL=true;
    bool firstcontactR=true;
    bool fullcontactL=false;
    bool fullcontactR=false;
    while (ros::ok())
    {

        //  for robot test musbe uncommented

        if (qc_initial_bool) {
            ROS_INFO_ONCE("qc is initializing!");
            ros::spinOnce();
            continue;
        }

        if(wrench_init_bool){
            WrenchHomming();
            for(int  i = 0;i < 12;i++){msg.data.push_back(qc_offset[i]);}
            for(int  i = 12;i < 28;i++){msg.data.push_back(0);}
            chatter_pub.publish(msg);
            ros::spinOnce();
            loop_rate.sleep();
            continue;
        }
if(true){

        if(OnlineTaskSpace.localTiming<.1){contact_flag_timing=-contact_flag_timing;}//flag to show expected contact according to timing,sign of flag changes

        //-------------for detecting the first contact of Left foot with ground-------------//
        //-------------flag to show contact detected by sensors,sign of flag changes-------------//
        if (( (a)<footSensorthreshold && (b)<footSensorthreshold && (c)<footSensorthreshold && (d)<footSensorthreshold)){
            fullcontactL=false;
            firstcontactL=true;
        }
        // if (firstcontact==true &&( (a)>=footSensorSaturation || (b)>=footSensorSaturation || (c)>=footSensorSaturation || (d)>=footSensorSaturation)){
        if (firstcontactL==true &&( (a)>=footSensorthreshold || (b)>=footSensorthreshold || (c)>=footSensorthreshold || (d)>=footSensorthreshold)){

            contact_flag_sensor2=-contact_flag_sensor2;
            firstcontactL=false;
            ROS_INFO("shalap [%f  %f] a=%d b=%d c=%d d=%d", OnlineTaskSpace.localTiming,OnlineTaskSpace.globalTime,a,b,c,d);
        }
        //-------------for detecting the full contact of Left foot with ground-------------//
        //-------------flag to show contact detected by sensors,sign of flag changes-------------//
        if ((!fullcontactL) && (a)>=footSensorSaturation && (b)>=footSensorSaturation && (c)>=footSensorSaturation && (d)>=footSensorSaturation){
            contact_flag_sensor=-contact_flag_sensor;
            ROS_INFO("left swing foot landing is successful = [%f  %f] a=%d b=%d c=%d d=%d", OnlineTaskSpace.localTiming,OnlineTaskSpace.globalTime,a,b,c,d);
            fullcontactL=true;
        }

        //-------------for detecting the first contact of Right foot with ground-------------//
        //-------------flag to show contact detected by sensors,sign of flag changes-------------//
        if (( (e)<footSensorthreshold && (f)<footSensorthreshold && (g)<footSensorthreshold && (h)<footSensorthreshold)){
            fullcontactR=false;
            firstcontactR=true;
        }
        // if (firstcontact==true && ( (e)>=footSensorSaturation || (f)>=footSensorSaturation || (g)>=footSensorSaturation || (h)>=footSensorSaturation)){
        if (firstcontactR==true &&( (e)>=footSensorthreshold || (f)>=footSensorthreshold || (g)>=footSensorthreshold || (h)>=footSensorthreshold)){

            contact_flag_sensor2=-contact_flag_sensor2;
            firstcontactR=false;
            ROS_INFO("shooloop[%f  %f]  e=%d f=%d g=%d h=%d", OnlineTaskSpace.localTiming,OnlineTaskSpace.globalTime,e,f,g,h);
        }

        //-------------for detecting the full contact of Right foot with ground-------------//
        if ((!fullcontactR) && (e)>=footSensorSaturation && (f)>=footSensorSaturation && (g)>=footSensorSaturation && (h)>=footSensorSaturation){
            contact_flag_sensor=-contact_flag_sensor;
            ROS_INFO("Right swing foot landing is successful= [%f  %f]  e=%d f=%d g=%d h=%d", OnlineTaskSpace.localTiming,OnlineTaskSpace.globalTime,e,f,g,h);
            fullcontactR=true;
        }

}

      //  StartPhase();

        //-----------------------------------------------------------------------------------------------------//
        //------------------------------- main loop of cyclic walking -----------------------------------------//
        //-----------------------------------------------------------------------------------------------------//
if (GlobalTime<DurationOfStartPhase ){
    for (int i = 0; i < 12; ++i) {
      SURENA.Links[i].JointAngle=0;
    }
    GlobalTime=GlobalTime+OnlineTaskSpace._timeStep;}

if (GlobalTime>DurationOfStartPhase ){
    GlobalTime=GlobalTime+OnlineTaskSpace._timeStep;

    if(true){//lastcode
//            //Ankle Trajectory Replacement
//            double m1;//x_al
//            double m2;//y_al
//            double m3;//z_al
//            double m4;//pitch_al
//            double m5;//x_ar
//            double m6;//y_ar
//            double m7;//z_ar
//            double m8;//pitch_ar

//            GlobalTime=GlobalTime+OnlineTaskSpace._timeStep;

//            if ((OnlineTaskSpace.StepNumber==1) && (OnlineTaskSpace.localTiming>=OnlineTaskSpace.TStart) ) {
//                OnlineTaskSpace.localTiming=OnlineTaskSpace._timeStep;//0.001999999999000000;
//                OnlineTaskSpace.localtimingInteger=1;
//                OnlineTaskSpace.StepNumber=OnlineTaskSpace.StepNumber+1;
//                //   KLtemp=false;

//            }

//            else if ((OnlineTaskSpace.localtimingInteger>=NumberOfTimeStep) &&   (OnlineTaskSpace.StepNumber>1    &&   OnlineTaskSpace.StepNumber<(OnlineTaskSpace.NStep+2))) {
//                OnlineTaskSpace.StepNumber=OnlineTaskSpace.StepNumber+1;
//                OnlineTaskSpace.localTiming=OnlineTaskSpace._timeStep;//0.001999999999000000;
//                OnlineTaskSpace.localtimingInteger=1;

//            }

//            OnlineTaskSpace.currentLeftFootX2=links[12].PositionInWorldCoordinate(0);
//            OnlineTaskSpace.currentLeftFootY2=links[12].PositionInWorldCoordinate(1);
//            OnlineTaskSpace.currentLeftFootZ=links[12].PositionInWorldCoordinate(2);

//            OnlineTaskSpace.currentRightFootX2=links[6].PositionInWorldCoordinate(0);
//            OnlineTaskSpace.currentRightFootY2=links[6].PositionInWorldCoordinate(1);
//            OnlineTaskSpace.currentRightFootZ=links[6].PositionInWorldCoordinate(2);

//            MatrixXd m=OnlineTaskSpace.AnkleTrajectory(OnlineTaskSpace.globalTime,OnlineTaskSpace.StepNumber,OnlineTaskSpace.localTiming);

//            m1=m(0,0); m2=m(1,0); m3=m(2,0); m4=m(3,0);
//            m5=m(4,0); m6=m(5,0); m7=m(6,0); m8=m(7,0);
//            //ROS_INFO("g.t=%f,l.t=%f",OnlineTaskSpace.globalTime,OnlineTaskSpace.localTiming);
//            //            ROS_INFO("m1=%f,m2=%f,m3=%f,m4=%f,m5=%f,m6=%f,m7=%f,m8=%f,",m1,m2,m3,m4,m5,m6,m7,m8);

//            RollModified=OnlineTaskSpace.RollAngleModification(OnlineTaskSpace.globalTime);
//            P=OnlineTaskSpace.PelvisTrajectory (OnlineTaskSpace.globalTime);

//            OnlineTaskSpace.globalTime=OnlineTaskSpace.globalTime+OnlineTaskSpace._timeStep;
//            OnlineTaskSpace.localTiming=OnlineTaskSpace.localTiming+OnlineTaskSpace._timeStep;
//            OnlineTaskSpace.localtimingInteger= OnlineTaskSpace.localtimingInteger+1;

//            if (round(OnlineTaskSpace.globalTime)<=round(OnlineTaskSpace.MotionTime)){

//                //if you want to have modification of height of pelvis please active the Pz(0,0) instead of P(2,0)
//                PoseRoot<<0,
//                        P(1,0),
//                        P(2,0),// Pz(0,0)
//                        0,
//                        0,
//                        0;

//                PoseRFoot<<0,
//                        m6,
//                        m7,
//                        0,
//                        -1*m8*(M_PI/180),
//                        0;

//                PoseLFoot<<0,
//                        m2,
//                        m3,
//                        0,
//                        -1*m4*(M_PI/180),
//                        0;

//                MatrixXd R_P(3,3);  MatrixXd R_F_L(3,3);    MatrixXd R_F_R(3,3);
//                R_P=MatrixXd::Identity(3,3);
//                R_F_L=MatrixXd::Identity(3,3);
//                R_F_R=MatrixXd::Identity(3,3);
//                double pelvis_roll=-(PoseRoot(1,0)/OnlineTaskSpace.Yd)*0*M_PI/180;//3 was good
//                R_P<<1,0,0,
//                      0,cos(pelvis_roll),-sin(pelvis_roll),
//                        0,sin(pelvis_roll),cos(pelvis_roll);

//                double pelvis_pitch=-asin(7/4*(max(m1,m5)-P(0,0)))*0;
//MatrixXd R_P2(3,3);
//R_P2<<cos(pelvis_pitch),0,sin(pelvis_pitch),
//        0,1,0,
//        -sin(pelvis_pitch),0,cos(pelvis_pitch);

//R_P=R_P*R_P2;
////                R_F_L<<cos(pelvis_roll),-sin(pelvis_roll),0,
////                        sin(pelvis_roll),cos(pelvis_roll),0,
////                        0,0,1;
////                R_F_R<<cos(pelvis_roll),-sin(pelvis_roll),0,
////                        sin(pelvis_roll),cos(pelvis_roll),0,
////                        0,0,1;

//                SURENA.doIK("LLeg_AnkleR_J6",PoseLFoot,R_F_L,"Body", PoseRoot,R_P);
//                SURENA.doIK("RLeg_AnkleR_J6",PoseRFoot,R_F_R,"Body", PoseRoot,R_P);

                //SURENA.ForwardKinematic(1);

                //*****************************
//                ROS_INFO("Root: x=%f,y=%f ,left: x=%f,y=%f  ,right: x=%f,y=%f",
//                         PoseRoot(0,0),PoseRoot(1,0),PoseLFoot(0,0),PoseLFoot(1,0),PoseRFoot(0,0),PoseRFoot(1,0));
//                double TurningRadius,yaw_al,yaw_ar,yaw_p;
//                TurningRadius=.3;

//                yaw_p=PoseRoot(0,0)/TurningRadius;
//                double sp=sin(yaw_p);
//                double cp=cos(yaw_p);
//                PoseRoot(0,0)=(TurningRadius-PoseRoot(1,0))*sp;
//                PoseRoot(1,0)=TurningRadius-(TurningRadius-PoseRoot(1,0))*cp;

//                yaw_al=PoseLFoot(0,0)/TurningRadius;
//                double s_al=sin(yaw_al);
//                double c_al=cos(yaw_al);
//                PoseLFoot(0,0)=(TurningRadius-PoseLFoot(1,0))*s_al;
//                PoseLFoot(1,0)=TurningRadius-(TurningRadius-PoseLFoot(1,0))*c_al;

//                yaw_ar=PoseRFoot(0,0)/TurningRadius;
//                double s_ar=sin(yaw_ar);
//                double c_ar=cos(yaw_ar);
//                PoseRFoot(0,0)=(TurningRadius-PoseRFoot(1,0))*s_ar;
//                PoseRFoot(1,0)=TurningRadius-(TurningRadius-PoseRFoot(1,0))*c_ar;

////                ROS_INFO("Root: x=%f,y=%f ,left: x=%f,y=%f  ,right: x=%f,y=%f\nyaw_p=%f,yaw_al=%f,yaw_al=%f",
////                         PoseRoot(0,0),PoseRoot(1,0),PoseLFoot(0,0),PoseLFoot(1,0),PoseRFoot(0,0),PoseRFoot(1,0),yaw_p,yaw_al,yaw_ar);
//                R_P<<cos(yaw_p),-sin(yaw_p),0,
//                        sin(yaw_p),cos(yaw_p),0,
//                        0,0,1;
//                R_F_R<<cos(yaw_ar),-sin(yaw_ar),0,
//                        sin(yaw_ar),cos(yaw_ar),0,
//                        0,0,1;
//                R_F_L<<cos(yaw_al),-sin(yaw_al),0,
//                        sin(yaw_al),cos(yaw_al),0,
//                        0,0,1;
//                //*--**********************************************

//                //                double th=10*M_PI/180;
//                //                R_P<<1,0,0,
//                //                        0,cos(th),-sin(th),
//                //                        0,sin(th),cos(th);

//                SURENA_turning.doIK("LLeg_AnkleR_J6",PoseLFoot,R_F_L,"Body", PoseRoot,R_P);
//                SURENA_turning.doIK("RLeg_AnkleR_J6",PoseRFoot,R_F_R,"Body", PoseRoot,R_P);
//                //SURENA_turning.ForwardKinematic(1);
//}

}

    double T_move=3;.7;
    double T_rest=5;
    double T_back=3;
    double periodTime=2*T_move+2*T_rest+2*T_back;
    double t_local=fmod(GlobalTime-DurationOfStartPhase,periodTime);
    double max=10*M_PI/180;
    double max_a=10*M_PI/180;
    double max_pl=2*M_PI/180;
    double max_pr=2*M_PI/180;
    double max_p=20*M_PI/180;

    double theta=0;
    double psi_l = 0;
    double psi_r = 0;
    double phi_r = 0;
    double phi_l = 0;

    if(t_local<periodTime/2){theta=move_rest_back(max,t_local,0,T_move,T_rest,T_back);}
    else if(t_local<periodTime){theta=move_rest_back(-max,t_local,periodTime/2,T_move,T_rest,T_back);}

    psi_l=move_rest_back(max_pl,t_local,0,T_move,T_rest,T_back);
    psi_r=move_rest_back(max_pr,t_local,periodTime/2,T_move,T_rest,T_back);

    phi_r=move_rest_back(max_p,t_local,T_move,T_rest/2,0,T_rest);
    phi_l=move_rest_back(max_p,t_local,periodTime/2+T_move,T_rest/2,0,T_rest);

    SURENA.Links[2].JointAngle=-theta-psi_r;
    SURENA.Links[6].JointAngle=saturate(theta,-max_a,max_a);
    SURENA.Links[8].JointAngle=-theta+psi_l;
    SURENA.Links[12].JointAngle=saturate(theta,-max_a,max_a);

    SURENA.Links[3].JointAngle=-phi_r/2;
    SURENA.Links[4].JointAngle=phi_r;
    SURENA.Links[5].JointAngle=-phi_r/2;

    SURENA.Links[9].JointAngle=-phi_l/2;
    SURENA.Links[10].JointAngle=phi_l;
    SURENA.Links[11].JointAngle=-phi_l/2;

        }
      //  ankleAdaptation();

         links = SURENA.GetLinks();
        //links = SURENA_turning.GetLinks();
//hand_move();
        //EndPhase();

        //        if(links[5].JointAngle<min_test){min_test=links[5].JointAngle;}
        //        if(links[5].JointAngle>max_test){max_test=links[5].JointAngle;}

        double ankle_adaptation_switch=0;// 1 for activating adaptation 0 for siktiring adaptation
        double k_roll_r=0;
        double k_roll_l=0;
        double k_pitch=0;
        cntrl[0]=0.0;
        cntrl[1]=links[1].JointAngle;
        cntrl[2]=links[2].JointAngle+k_roll_r*RollModified(0,0);//+k_roll_corr*(links[2].JointAngle-roll_absoulte[0])
        cntrl[3]=links[3].JointAngle+k_pitch*PitchModified;
        cntrl[4]=links[4].JointAngle;
        cntrl[5]=saturate(links[5].JointAngle,-M_PI/5,M_PI/4)+ankle_adaptation_switch*teta_motor_R;//pitch
        cntrl[6]=links[6].JointAngle+ankle_adaptation_switch*(phi_motor_R);//roll
        cntrl[7]=links[7].JointAngle;
        cntrl[8]=links[8].JointAngle+k_roll_l*RollModified(1,0);//+k_roll_corr*(links[8].JointAngle-roll_absoulte[1])
        cntrl[9]=links[9].JointAngle+k_pitch*PitchModified;
        cntrl[10]=links[10].JointAngle;
        cntrl[11]=saturate(links[11].JointAngle,-M_PI/5,M_PI/4)+ankle_adaptation_switch*teta_motor_L;
        cntrl[12]=links[12].JointAngle+ankle_adaptation_switch*(phi_motor_L);

        vector<int> qref(12);
        qref=QC.ctrldata2qc(cntrl);

        msg.data.clear();
        bool left_first=true;//right support in first step
        if(left_first){
            for(int  i = 0;i < 12;i++)
            {
                msg.data.push_back(qref[i]+qc_offset[i]);
            }}
        else{
            msg.data.push_back(-qref[5]+qc_offset[0]);
            msg.data.push_back(-qref[4]+qc_offset[1]);
            msg.data.push_back(-qref[6]+qc_offset[2]);
            msg.data.push_back(-qref[7]+qc_offset[3]);
            msg.data.push_back(-qref[1]+qc_offset[4]);
            msg.data.push_back(-qref[0]+qc_offset[5]);
            msg.data.push_back(-qref[2]+qc_offset[6]);
            msg.data.push_back(-qref[3]+qc_offset[7]);
            msg.data.push_back(-qref[11]+qc_offset[8]);
            msg.data.push_back(-qref[10]+qc_offset[9]);
            msg.data.push_back(-qref[9]+qc_offset[10]);
            msg.data.push_back(-qref[8]+qc_offset[11]);
        }

        for(int  i = 12;i < 28;i++)
        {
            msg.data.push_back(0);
        }

        chatter_pub.publish(msg);

        if (simulation){SendGazebo(links,0*RollModified,0*PitchModified,teta_motor_R,phi_motor_R,teta_motor_L,phi_motor_L);}
        links = SURENA.GetLinks();

        msg_contact_flag.data.clear();
        msg_contact_flag.data.push_back(contact_flag_timing);
        msg_contact_flag.data.push_back(contact_flag_sensor);
        msg_contact_flag.data.push_back(contact_flag_sensor2);

        contact_flag.publish(msg_contact_flag);

        if(count%20==0){ //use to print once in n steps
            // ROS_INFO("");
            //            ROS_INFO("I heard data of sensors :t=%f [%d %d %d %d] & [%d %d %d %d]",OnlineTaskSpace.globalTime,a,b,c,d,e,f,g,h);
            //ROS_INFO("I heard a b c d: [%d  %d %d %d],e f g h: [%d  %d %d %d]", a,b,c,d,e,f,g,h);
                     //    ROS_INFO("teta_motor_L=%f,teta_motor_R=%f,phi_motor_L=%f,phi_motor_R=%f",teta_motor_L,teta_motor_R,phi_motor_L,phi_motor_R);
            //   ROS_INFO("ankl pith min=%f,max=%f",min_test*180/M_PI,max_test*180/M_PI);

        }

      //  if(GlobalTime>=DurationOfStartPhase+OnlineTaskSpace.TStart+OnlineTaskSpace.Tc+OnlineTaskSpace.TDs){break;}

        ros::spinOnce();
        loop_rate.sleep();
        ++count;
    }

    return 0;
}


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file pbes_test.cpp
/// \brief Add your file description here.

#include <string>
#include <boost/test/minimal.hpp>
#include "mcrl2/pbes/txt2pbes.h"

using namespace mcrl2;
using namespace mcrl2::pbes_system;

const std::string PBESSPEC1 =
  "pbes nu X(b: Bool) = exists n: Nat. Y(n) && val(b); \n"
  "     mu Y(n: Nat)  = X(n >= 10);                    \n"
  "                                                    \n"
  "init X(true);                                       \n"
  ;

const std::string PBESSPEC2 =
  "sort DATA = struct d1 | d2;                         \n"
  "     Enum3 = struct e2_3 | e1_3 | e0_3;             \n"
  "     Frame = struct frame(getd: DATA, getb: DATA);  \n"
  "                                                    \n"
  "glob dc: Frame;                                     \n"
  "                                                    \n"
  "pbes nu X(s30_K: Pos, f_K: Frame) =                 \n"
  "       X(1, f_K);                                   \n"
  "                                                    \n"
  "init X(1, dc);                                      \n"
  ;

void test_txt2pbes()
{
  pbes p;
  p = txt2pbes(PBESSPEC1);
  BOOST_CHECK(p.is_well_typed());
  p = txt2pbes(PBESSPEC2);
  BOOST_CHECK(p.is_well_typed());
}

int test_main(int argc, char* argv[])
{
  test_txt2pbes();

  return 0;
}