/home/slemaign/softs-local/BioMove3D-git/lightPlanner/proto/Manipulation.h

#ifndef __MANIPULATION_H__
#define __MANIPULATION_H__

#if defined (GRASP_PLANNING) && defined (LIGHT_PLANNER)

#include <vector>
#include <map>

#include "GraspPlanning-pkg.h"

#include "P3d-pkg.h"
#include <list>

#include "ManipulationStruct.h"
#include "ManipulationUtils.hpp"

class DoubleGraspData{
  public:
  DoubleGraspData(p3d_rob* robot){
    _robot = robot;
    _config = p3d_alloc_config(robot);
  }
  DoubleGraspData(p3d_rob* robot, gpDoubleGrasp dGrasp, configPt cCConfig){
    _robot = robot;
    _doubleGrasp = dGrasp;
    _config = cCConfig;
  }
  virtual ~DoubleGraspData(){
    p3d_destroy_config(_robot, _config);
  }
  inline gpDoubleGrasp getDoubleGrasp(){
    return _doubleGrasp;
  }
  inline void setDoubleGrasp(gpDoubleGrasp doubleGrasp){
    _doubleGrasp = doubleGrasp;
  }
  inline configPt getConfig(){
    return _config;
  }
  inline void setConfig(configPt cCConfig){
    _config = cCConfig;
  }
  inline void setObjectExchangeMat(p3d_matrix4 objectExchangeMat){
    p3d_mat4Copy(objectExchangeMat, _objectExchangeMat);
  }
  inline void getObjectExchangeMat(p3d_matrix4 objectExchangeMat){
    p3d_mat4Copy(_objectExchangeMat, objectExchangeMat);
  }
  private:
  p3d_rob* _robot;
  gpDoubleGrasp _doubleGrasp;
  p3d_matrix4 _objectExchangeMat;
  configPt _config;
};

class Manipulation{
  public :
    Manipulation(p3d_rob *robot);
    virtual ~Manipulation();
    void clear();
    void computeOfflineRoadmap(); 
    p3d_traj* computeRegraspTask(configPt startConfig, configPt gotoConfig, std::string offlineFile);
    p3d_traj* computeRegraspTask(configPt startConfig, configPt gotoConfig, std::string offlineFile, int whichTest);
  
    int findAllArmsGraspsConfigs(p3d_matrix4 objectStartPos, p3d_matrix4 objectEndPos);
    int findAllSpecificArmGraspsConfigs(int armId, p3d_matrix4 objectPos);
    double getCollisionFreeGraspAndApproach(p3d_matrix4 objectPos, gpHand_properties handProp, gpGrasp grasp, int whichArm, p3d_matrix4 tAtt, configPt* graspConfig, configPt* approachConfig);
    void computeExchangeMat(configPt startConfig, configPt gotoConfig);
    void computeDoubleGraspConfigList();
    inline void setExchangeMat(p3d_matrix4 exchangeMat){
      p3d_mat4Copy(exchangeMat, _exchangeMat);
    }
    inline void getExchangeMat(p3d_matrix4 exchangeMat){
      p3d_mat4Copy(_exchangeMat, exchangeMat);
    }
    void drawSimpleGraspConfigs();
    void drawDoubleGraspConfigs();
    void printStatDatas();
  
  protected:
    double getRobotGraspArmCost(gpGrasp grasp, configPt q);
    int getCollisionFreeDoubleGraspAndApproach(p3d_matrix4 objectPos, std::vector<gpHand_properties> armsProp, gpDoubleGrasp doubleGrasp, configPt* doubleGraspConfig);
    std::vector<gpHand_properties> InitHandProp(int armId);
    std::list<gpGrasp>* getGraspListFromMap(int armId);
    int checkTraj(p3d_traj * traj, p3d_graph* graph);
  
  private :
    std::map < int, std::map<int, ManipulationData*, std::less<int> >, std::less<int> > _handsGraspsConfig;
    std::list<DoubleGraspData*> _handsDoubleGraspsConfigs;
    p3d_rob * _robot;
    p3d_graph * _offlineGraph;
    static const int _maxColGrasps = 10;
    p3d_matrix4 _exchangeMat;
    std::vector<std::vector<double> > _statDatas;
};



class  Manipulation_JIDO {
  private :
     p3d_rob * _robotPt; 
     p3d_rob * _hand_robotPt;  
     //gpHand_type _handType;
     gpHand_properties _handProp;  
     std::list<gpGrasp> _graspList; 
     gpGrasp _grasp;   
     unsigned int _graspID; 
     gpPlacement _placement;   
     p3d_jnt *_cameraJnt; 
     double _cameraFOV; 
     int _cameraImageWidth, _cameraImageHeight; 
     configPt _configStart;
     configPt _configGoto;

     std::vector<configPt> _configTraj; 
     double _liftUpDistance;  
     int _nbGraspsToTestForPickGoto; 

     double _placementTranslationStep; 
     double _placementNbOrientations; 
     double _placementOffset; 
     double _QCUR[6];
     double _QGOAL[6];
     double _XCUR[6];
     double _XGOAL[6];
     double _qrest[6];  
     p3d_rob *_object; 
     p3d_rob *_support; 
     p3d_rob *_human; 
     std::list<gpPlacement> _placementList; 
     std::list<gpPlacement> _placementOnSupportList; 
     bool _capture;
     bool _cartesian; 
     bool _objectGrabed; // not used for now (redundent with robot->isCarryingObject)
 public:
     bool displayGrasps; 
     bool displayPlacements; 
     std::vector < std::vector <double> > positions;
     std::vector <int> lp;
 public :
     Manipulation_JIDO(p3d_rob * robotPt, gpHand_type handType);
     virtual ~Manipulation_JIDO();
     void clear();

     configPt robotStart();
     configPt robotGoto();
     configPt robotRest();

     void setCapture(bool v);
     bool getCapture();
     int centerCamera();
     int forbidWindowEvents();
     int allowWindowEvents();
     /*Functions relative to JIDO */
     int setArmQ(double q1, double q2, double q3, double q4, double q5, double q6);
     int getArmQ(double *q1, double *q2, double *q3, double *q4, double *q5, double *q6);
     int setArmX(double x, double y, double z, double rx, double ry, double rz);
     int setArmX(double x, double y, double z, unsigned int nbTries= 30);
     int getArmX(double* x, double* y, double* z, double* rx, double* ry, double* rz);
     void setArmCartesian(bool v);
     bool getArmCartesian();
     int setArmTask(MANIPULATION_TASK_TYPE_STR t);
     int setObjectToManipulate(char *objectName);
     int setSupport(char *supportName);
     int setHuman(char *humanName);
     int setCameraJnt(char *cameraJntName);
     int setCameraFOV(double fov);
     int setCameraImageSize(int width, int height);
     int setNbGraspsToTestForPickGoto(int n);
     int reduceGraspList(int maxSize);
     int printConstraintInfo();
     int setPoseWrtEndEffector(double x, double y, double z, double rx, double ry, double rz, configPt q);
     int dynamicGrasping(char *robot_name, char *hand_robot_name, char *object_name);
     int robotBaseGraspConfig(char *objectName, double *x, double *y, double *theta);
     MANIPULATION_TASK_MESSAGE armPlanTask(MANIPULATION_TASK_TYPE_STR task, configPt qStart, configPt qGoal, char* objectName, std::vector <int> &lp, std::vector < std::vector <double> > &positions);

  
     int armComputePRM();
     
     int cleanRoadmap();
     int cleanTraj();

     int grabObject(char* objectName);
     int releaseObject();

     void draw();
 
     int setLiftUpDistance(double dist) {
       _liftUpDistance= fabs(dist); 
       return 0;
     }
     double liftUpDistance() {
       return _liftUpDistance;
     }
     
     /* Functions relative to other robots */
     int setFreeflyerPose(p3d_rob *robotPt, double x, double y, double z, double rx, double ry, double rz);
     int getObjectPose(p3d_matrix4 pose);
     int getObjectPose(double *x, double *y, double *z, double *rx, double *ry, double *rz);
     int setFreeflyerPoseByName(char *name, double x, double y, double z, double rx, double ry, double rz);
     p3d_obj * getObjectByName(char *object_name);
     int setObjectPoseWrtEndEffector(double x, double y, double z, double rx, double ry, double rz);

     /* Functions relative to object grasping */
    int findPregraspAndGraspConfiguration(double distance, double *pre_q1, double *pre_q2, double *pre_q3, double *pre_q4, double *pre_q5, double *pre_q6, double *q1, double *q2, double *q3, double *q4, double *q5, double *q6);
    bool isObjectGraspable(char *objectName);

     /* Functions relative to object placement */
    int findPlacementConfigurations(double distance, double *pre_q1, double *pre_q2, double *pre_q3, double *pre_q4, double *pre_q5, double *pre_q6, double *q1, double *q2, double *q3, double *q4, double *q5, double *q6);

    int addConfigTraj(configPt config);
    int clearConfigTraj();
    int copyConfigTrajToFORM();
    int destroyTrajectories();
    int checkCollisionOnTraj();
    int checkCollisionOnTraj(int currentLpId);
    int replanCollidingTraj(int currentLpId, std::vector <int> &lp, std::vector < std::vector <double> > &positions);
  protected:
     /*Functions relative to JIDO */
     int computeTrajBetweenTwoConfigs(bool cartesian, configPt qi, configPt qf);
     int computeGraspList();
     int findSimpleGraspConfiguration(double *q1, double *q2, double *q3, double *q4, double *q5, double *q6);
     int computePlacementList();
     int markGraspAsTested(int id);

     int computeRRT();
     int computeOptimTraj();
};

#endif

#endif