/home/slemaign/softs-local/BioMove3D-git/lightPlanner/proto/ManipulationPlanner.hpp

#ifndef __MANIPULATIONPLANNER_HPP__
#define __MANIPULATIONPLANNER_HPP__

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

#include "P3d-pkg.h"
#include "Localpath-pkg.h"

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

#include <map>

class  ManipulationPlanner {
  public:
  /* ******************************* */
  /* ******* (Con)Destructor ******* */
  /* ******************************* */
    ManipulationPlanner(p3d_rob * robot);
    virtual ~ManipulationPlanner();

  /* ******************************* */
  /* *********** Cleaning ********** */
  /* ******************************* */
    void clear();
    int cleanRoadmap();
    int cleanTraj();

  /* ******************************* */
  /* ******* (Ge)Setters *********** */
  /* ******************************* */
    void setDebugMode(bool value);
#ifdef MULTILOCALPATH
    void setDebugSoftMotionMode(bool value);
    
  int getBaseMLP() { return _BaseMLP; } 
  int getHeadMLP() { return _HeadMLP; }
  int getUpBodyMLP() { return _UpBodyMLP; }
  int getUpBodysmMLP() { return _UpBodySmMLP; }
#endif
  
  // Set and reset the planning and smoothing
  // Functions used by the Manipulation planner
  void setPlanningMethod(p3d_traj* (*funct)(p3d_rob* robot, configPt qs, configPt qg));
  void resetPlanningMethod();
  void setSmoothingMethod(void (*funct)(p3d_rob* robot, p3d_traj* traj, int nbSteps, double maxTime));
  void resetSmoothingMethod();
  void setReplanningMethod(p3d_traj* (*funct)(p3d_rob* robotPt, p3d_traj* traj, p3d_vector3 target, int deformationViaPoint));
  //void resetReplanningMethod();
  
    void setPlanningTime(double time);
    double getPlanningTime(void) const;
  
    void setOptimizeSteps(int nbSteps);
    int getOptimizeSteps(void) const;

    void setOptimizeTime(double time);
    double getOptimizeTime(void) const;

    void setSafetyDistanceValue(double value);
    double getSafetyDistanceValue(void) const;
  
    void setPlacementTry(int nbTry);
    int getPlacementTry(void);
    
    void setRobotPath(p3d_traj* path) { _robotPath = _robot->tcur; } 
  
    inline p3d_rob* robot()  const{return _robot;}

    inline configPt robotStart() const{if (_robot != NULL) {return _robot->ROBOT_POS;} else {return NULL;}}
    inline configPt robotGoto() const{if (_robot != NULL) {return _robot->ROBOT_GOTO;} else {return NULL;}}
        

    inline ManipulationData& getManipulationData() {return _configs;}
    inline ManipulationConfigs getManipulationConfigs()  const {return _manipConf;}
        /* ******************************* */
  /* ******* Hands / Grasping ****** */
  /* ******************************* */
    MANIPULATION_TASK_MESSAGE computeManipulationData(int armId,p3d_rob* object, gpGrasp& grasp);
    MANIPULATION_TASK_MESSAGE findArmGraspsConfigs(int armId, p3d_rob* object, gpGrasp& grasp, ManipulationData& configs);
    MANIPULATION_TASK_MESSAGE getGraspOpenApproachExtractConfs(p3d_rob* object, int armId, gpGrasp& grasp, p3d_matrix4 tAtt, ManipulationData& configs) const;
    MANIPULATION_TASK_MESSAGE getHoldingOpenApproachExtractConfs(p3d_rob* object, std::vector<double> &objGoto, p3d_rob* placement, int armId, gpGrasp& grasp, p3d_matrix4 tAtt,  ManipulationData& configs) const;
        /* ******************************* */
  /* ******* Planning Modes ******** */
  /* ******************************* */
    void checkConfigForCartesianMode(configPt q, p3d_rob* object);
    void setArmCartesian(int armId, bool cartesian);
    bool getArmCartesian(int armId) const;


  /* ******************************* */
  /* ******* Motion Planning ******* */
  /* ******************************* */
    MANIPULATION_TASK_MESSAGE cutTrajInSmall ( p3d_traj* inputTraj, p3d_traj* outputTraj );
    MANIPULATION_TASK_MESSAGE concatTrajectories (std::vector<p3d_traj*>& trajs, p3d_traj** concatTraj);
    void fitConfigurationToRobotBounds(configPt q);
    MANIPULATION_TASK_MESSAGE computeRRT(int smoothingSteps, double smootingTime, bool biDir);
    /* TODO Add CXX PLanner computation*/
    MANIPULATION_TASK_MESSAGE armComputePRM(double ComputeTime);
    p3d_traj* computeTrajBetweenTwoConfigs(configPt qi, configPt qf, MANIPULATION_TASK_MESSAGE* status);
#ifdef MULTILOCALPATH

    int computeSoftMotion(p3d_traj* traj, MANPIPULATION_TRAJECTORY_CONF_STR &confs, SM_TRAJ &smTraj);
#endif

    MANIPULATION_TASK_MESSAGE armToFreePoint(int armId, configPt qStart, std::vector<double> &objGoto, p3d_rob* object, std::vector <p3d_traj*> &trajs);
    MANIPULATION_TASK_MESSAGE armExtract(int armId, configPt qStart, p3d_rob* object, std::vector <p3d_traj*> &trajs);
    MANIPULATION_TASK_MESSAGE armToFree(int armId, configPt qStart, configPt qGoal, bool useSafetyDistance, p3d_rob* object, std::vector <p3d_traj*> &trajs);
    
    MANIPULATION_TASK_MESSAGE armPickGoto(int armId, configPt qStart, p3d_rob* object, gpGrasp& grasp, std::vector <p3d_traj*> &trajs);
    MANIPULATION_TASK_MESSAGE armPickGoto(int armId, configPt qStart, p3d_rob* object, configPt graspConfig, configPt openConfig, configPt approachFreeConfig, std::vector <p3d_traj*> &trajs);

    MANIPULATION_TASK_MESSAGE armPickTakeToFreePoint(int armId, configPt qStart, std::vector<double> &objGoto, p3d_rob* object, p3d_rob* support, std::vector <p3d_traj*> &trajs);
    MANIPULATION_TASK_MESSAGE armPickTakeToFree(int armId, configPt qStart, configPt qGoal, p3d_rob* object, p3d_rob* support, std::vector <p3d_traj*> &trajs);
    MANIPULATION_TASK_MESSAGE armPickTakeToFree(int armId, configPt qStart, configPt qGoal, p3d_rob* object, p3d_rob* support, configPt approachGraspConfig, gpGrasp &grasp, std::vector <p3d_traj*> &trajs);

    MANIPULATION_TASK_MESSAGE armTakeToPlace(int armId, configPt qStart, p3d_rob* object, p3d_rob* support, p3d_rob* placement, std::vector <p3d_traj*> &trajs);
    MANIPULATION_TASK_MESSAGE armTakeToPlace(int armId, configPt qStart, p3d_rob* object, p3d_rob* support, std::vector<double> &objGoto, p3d_rob* placement, std::vector <p3d_traj*> &trajs);
    MANIPULATION_TASK_MESSAGE armTakeToPlace(int armId, configPt qStart, configPt approachGraspConfig, configPt approachGraspConfigPlacement, configPt qGoal, p3d_rob* object,  p3d_rob* support, p3d_rob* placement, std::vector <p3d_traj*> &trajs);

    MANIPULATION_TASK_MESSAGE armPlaceFromFree(int armId, configPt qStart, p3d_rob* object, p3d_rob* placement, std::vector <p3d_traj*> &trajs);
    MANIPULATION_TASK_MESSAGE armPlaceFromFree(int armId, configPt qStart, p3d_rob* object, std::vector<double> &objGoto, p3d_rob* placement, std::vector <p3d_traj*> &trajs);
    MANIPULATION_TASK_MESSAGE armPlaceFromFree(int armId, configPt qStart, p3d_rob* object, p3d_rob* placement, configPt approachGraspConfig, configPt depositConfig, std::vector <p3d_traj*> &trajs);

    MANIPULATION_TASK_MESSAGE armEscapeObject(int armId, configPt qStart, p3d_rob* object, std::vector <p3d_traj*> &trajs);
    MANIPULATION_TASK_MESSAGE armEscapeObject(int armId, configPt qStart, configPt openGraspConfig, configPt approachFreeConfig, p3d_rob* object, std::vector <p3d_traj*> &trajs);
  /* ******************************* */
  /* ******** Task Planning ******** */
  /* ******************************* */
    MANIPULATION_TASK_MESSAGE armReplan(p3d_vector3 target, int qSwitchId, SM_TRAJ &smTrajs);
  
    MANIPULATION_TASK_MESSAGE armPlanTask(MANIPULATION_TASK_TYPE_STR task, int armId, configPt qStart, configPt qGoal, std::vector<double> &objStart, std::vector<double> &objGoto, const char* objectName,  const char* supportName, const char* placementName, gpGrasp& grasp, std::vector <p3d_traj*> &trajs);
    
    MANIPULATION_TASK_MESSAGE armPlanTask(MANIPULATION_TASK_TYPE_STR task, int armId, configPt qStart, configPt qGoal, std::vector<double> &objStart, std::vector<double> &objGoto, const char* objectName,  const char* supportName, const char* placementName, std::vector <p3d_traj*> &trajs);

#ifdef MULTILOCALPATH

    MANIPULATION_TASK_MESSAGE armPlanTask(MANIPULATION_TASK_TYPE_STR task, int armId, configPt qStart, configPt qGoal, std::vector<double> &objStart, std::vector<double> &objGoto, const char* objectName,  const char* supportName, const char* placementName, gpGrasp& grasp, std::vector <MANPIPULATION_TRAJECTORY_CONF_STR> &confs, std::vector <SM_TRAJ> &smTrajs);
    MANIPULATION_TASK_MESSAGE armPlanTask(MANIPULATION_TASK_TYPE_STR task, int armId, configPt qStart, configPt qGoal, std::vector<double> &objStart, std::vector<double> &objGoto, const char* objectName, const char* supportName, const char* placementName, std::vector <MANPIPULATION_TRAJECTORY_CONF_STR> &confs, std::vector <SM_TRAJ> &smTrajs);
#endif
        
  private:
        
  p3d_rob * _robot;
    
  /* ******************************* */
  /* ******* Motion Planning ******* */
  /* ******************************* */
#ifdef MULTILOCALPATH

    int _BaseMLP;
    int _HeadMLP;
    int _UpBodyMLP;
    int _UpBodySmMLP;
#endif

    double _planningTime;
    int _optimizeSteps;
    double _optimizeTime;
    double _safetyDistanceValue;
    
    int _placementTry;
        
  /* ******************************* */
  /* *******  Manipulation Data **** */
  /* ******************************* */
        ManipulationData _configs;
  ManipulationConfigs _manipConf;
        
  /* ******************************* */
  /* ** Motion Planning funtions *** */
  /* ******************************* */
  
  p3d_traj* _robotPath;
  
  p3d_traj* (*_plannerMethod)(p3d_rob* robot, configPt qs, configPt qg);
  void (*_smoothingMethod)(p3d_rob* robot, p3d_traj* traj, int nbSteps, double maxTime);
  
  p3d_traj* (*_replanningMethod)(p3d_rob* robotPt, p3d_traj* traj, p3d_vector3 target, int deformationViaPoint);
  
};

#endif

#endif