ConfGenerator.h

#ifndef CONFGENERATOR_H
#define CONFGENERATOR_H

#include "API/Device/robot.hpp"
#include "planner/planner.hpp"

#include "utils/OtpUtils.hpp"
#include "utils/ConfGenerator.h"

#include "LightPlanner-pkg.h"
#include "planner/TrajectoryOptim/plannarTrajectorySmoothing.hpp"
#include "HRI_costspace/HRICS_Natural.hpp"



#include <Eigen/Core>
#include <Eigen/StdVector>


class constrainedLink
{
public:
    constrainedLink();
    constrainedLink(Robot* first, std::vector<std::vector<double> > matrixRot, std::vector<double> vectTrans);
    virtual ~constrainedLink();
    void addRange(std::pair<int,std::vector<std::pair<double,double> > > range);


    std::string name;
    Robot* R;
    std::vector<std::vector<double> > matrixRotation;
    std::vector<double> vectTranslation;

    std::map<int,std::vector<std::pair<double,double> > > rangeList;
    int nbDofs;
};


//typedef struct linkStruct
//{
//    Robot* first;
//    Robot* second;
//    configurationConstraint CC;
//}linked;

//typedef struct configurationConstraintStruct
//{
//
//}configurationConstraint;

class ConfGenerator
{
public:
  ConfGenerator();
  ConfGenerator(Robot* rob,Robot* human);

  void setHuman(Robot* human);
  void setRobot(Robot* rob);
  
  std::vector<Eigen::Vector3d> getOTPList(){return m_OTPList;}
  
  bool computeRobotIkForGrabing( configPt& q );
  
  bool computeRobotIkForGrabing( configPt& q, const Eigen::Vector3d& point );
  
  bool computeHandoverConfigFromList( std::pair<confPtr_t,confPtr_t>& best_handover_conf, double& best_cost );
  
  bool initialize( std::string filename, HRICS::Natural* reachableSpace );
  
  bool sortConfig(std::vector<HRICS::ConfigHR>& configList, int nbNode, bool isStanding, bool isSlice, HRICS::Natural* reachableSpace);
  
  void addToList(Eigen::Vector3d WSPoint);
  
  Eigen::Vector3d setCurOTP( Eigen::Vector3d WSPoint);

  std::vector<HRICS::ConfigHR> getConfList();
  
  void drawOTPList(bool value);
  
  bool placeRobot();
  
  std::vector<HRICS::ConfigHR> addConfToList();
  
  void removeLastConf();
  
  void clearConfList();
  
  void saveToXml(std::string filename);
  
  std::vector<HRICS::ConfigHR> loadFromXml(std::string filename);

  double getConfigCost(HRICS::Natural* reachableSpace);

  //########################################
  //new version :
  //########################################
  
  bool generateConfiguration(int nbConfs);
  bool addConstraintToList(constrainedLink CL);

  
private:
  
  std::pair<confPtr_t,confPtr_t> setRobotsToConf(const HRICS::ConfigHR& handover_conf);
  
  std::vector<Eigen::Vector3d> m_OTPList;
  
  std::vector<HRICS::ConfigHR> m_configList;
  
  Robot* _robot;
  Robot* _human;

  double _maxDist;

  //########################################
  //new version :
  //########################################
  std::vector<constrainedLink> linkList;
  std::map<std::string,constrainedLink> linkListFiltred;
};

#endif // CONFGENERATOR_H