taskManagerInterface.hpp

#ifndef TASKMANAGERINTERFACE_H
#define TASKMANAGERINTERFACE_H

#include "taskManager.hpp"

enum PlannerStates {
    Iddle,
    ExecuteTraj,
    ExecuteSubTraj,
    PlanningGTP,
    ComputeTraj,
    Smoothing,
    SmoothingPlan,
    FindingAlternative,
    ComputeList,
    ComputeListMP,
    ComputeHeuristic,
    ComputeMultiple,
    LaunchMSgConnector,
    testFunction
};


typedef struct struct_inputs {
    std::string typeStr;
    TasksId type;
    std::multimap<std::string,std::string> agents;
    std::multimap<std::string,std::string> objects;
    std::multimap<std::string,p3d_point> points;
    std::multimap<std::string,std::string> additionalData;
    bool MPComputation;
    TaskConstraints* constraints;

    bool isAlreadyComputed;
    int TaskId;
    int nbAlt;

    WorldState* ws;

}task_in;


class taskManagerInterface
{
    MOVE3D_STATIC_LOGGER;
public:
    taskManagerInterface();
    void initTaskManager();

    void setTypeStr(std::string str);
    void setType(TasksId type);
    TasksId getType() {return _type;}

    void setAgent(std::string id, std::string name);
    void setObject(std::string id, std::string name);
    void addPoint(std::string id, p3d_point p);
    void addAdditinalData(std::string id, std::string value);


    std::multimap<std::string,std::string> getAgents() {return _agents;}
    std::multimap<std::string,std::string> getObjects() {return _objects;}
    std::multimap<std::string,p3d_point> getPoints() {return _points;}
    std::multimap<std::string,std::string> getAdditionalData() {return _additionalData;}

    void desactiveConstraints(Robot* ag);
    void clearAgents() {_agents.clear();}
    void clearObjects() {_objects.clear();}
    void clearPoints() {_points.clear();}
    void clearAdditionalData() {_additionalData.clear();}

    void setWorldState(WorldState* WS);
    WorldState* getWorldState() {return _WS;}

    void callGTP();
    void callAlternative(int taskId);
    double getTaskHeuristic();

    TaskManager* getTaskManager() { return m_TM; }
    Task* getLastSolutionFound();

    PlannerStates getState() { return _PS; }
    void setState(PlannerStates PS) { _PS = PS; }

    int getSolutionId() {return _solutionId;}
    void setSolutionId(int solId) {_solutionId = solId;}

    int getAlternativeId() {return _alternativeId;}
    void setAlternativeId(int altId) {_alternativeId = altId;}

    int getSubSolutionId() {return _subSolutionId;}
    void setSubSolutionId(int subSolId) {_subSolutionId = subSolId;}

    void resetAll();
    void clearAllInputs();
    std::string getCurrentInputsDescr();

    TaskSubSolution* current_executing_traj;

    bool hasWorldState(){return _hasWorldState;}
    void setHasWorldState(bool val) {_hasWorldState = val;}

    TaskSolution* getCurrentSol() {return current_solution;}
    void setCurrentSol(TaskSolution* curSol) {current_solution = curSol;}

    PlanNode* getCurrentPN() {return _curr_PN;}
    void setCurrentPN(PlanNode* curPN) {_curr_PN = curPN;}

    TaskSubSolution* getCurrentSubSol() {return current_sub_solution;}
    void setCurrentSubSol(TaskSubSolution* curSol) {current_sub_solution = curSol;}

    bool getComputeMP() {return _computeMP;}
    void setComputeMP(bool CMP) { _computeMP = CMP;}

    TaskConstraints* getConstraints() {return _constraints;}
    void setConstraints(TaskConstraints* vt) {_constraints = vt;}

    void removeConstraintFromPrevTS(int taskId, int altId);

    void addCurrentTaskToList();
    void loadCurrentTaskFrom(task_in TI);
    void clearList();
    void resetAllFromScratch();
    void removeLastTaskFromList();
    std::string getOneTaskDescr(task_in t);
    std::string getListDescr();
    TaskConstraints* getGoalConstraints();
    bool testGoalConstraints();
    bool computePlan(bool computeMP, int init_id);
    bool computeJustMP();

    bool getWSFromRangeParam(int taskId, int altId, WorldState* ws, double rangeParam);

    std::vector<std::pair<std::pair<int,int>, double> > getBacktrackPriorities();

    std::vector<task_in> getTaskList() {return _tasksList;}
    void setTaskList(std::vector<task_in> ti) {_tasksList = ti;}

    bool getPlanFound() {return _planFound;}
    void setPlanFound(bool pf) {_planFound = pf;}

    bool getIsComputePlanSuccess() {return _isComputePlanSuccess;}
    void setIsComputePlanSuccess(bool s) {_isComputePlanSuccess = s;}

    WorldState* getInitWS() {return _initWS;}
    void setInitWS(WorldState* ws){_initWS = ws;}

    void savePlanToDisk(std::string path);
    void loadPlanFromDisk(std::string path);

    void readTask(std::vector<std::string> vt);
    TaskSubSolution* readTSS(TaskSolution *TS, std::vector<std::string> vt);
    std::pair<std::string,WorldState*> readWS(std::vector<std::string> vt);
    p3d_traj *readTraj(std::vector<std::string> vt, Robot *r);
    p3d_localpath *readLocalpath(std::vector<std::string> vt, std::string type, Robot *r);
    confPtr_t readConf(std::vector<std::string> ligne, Robot *r );


    double getCurrentHeurisitic() {return _currentHeuristic;}
    void setCurrentHeurisitic(double h) {_currentHeuristic = h;}

    bool addAttachementsFromTask(int taskId, int alternativeId);
    bool addAttachementFromObject(Robot* robot, Robot* object, int armId);
    void clearAttachements();
    std::vector<Attachement> getAttachements() {return _attachements;}


private:
    TaskManager* m_TM;
    TasksId _type;
    std::string _typeStr;
    std::multimap<std::string,std::string> _agents;
    std::multimap<std::string,std::string> _objects;
    std::multimap<std::string,p3d_point> _points;
    std::multimap<std::string,std::string> _additionalData;

    TaskConstraints* _constraints;

    bool _computeMP;

    WorldState* _WS;
    bool _hasWorldState;
    int _solutionId; // this is the task id, but to not confuse with taskId it s name solution ID
    int _alternativeId;
    int _subSolutionId;

    int current_computing_id;

    PlannerStates _PS;

    TaskSubSolution* current_sub_solution;

    TaskSolution* current_solution;
    PlanNode* _curr_PN;

    std::vector<task_in> _tasksList;
    bool _planFound;
    TaskConstraints* _goalConstraints;

    bool _isComputePlanSuccess;

    WorldState* _initWS;

    double _currentHeuristic;

    task_in _lastInputs;

    std::vector<Attachement> _attachements;


};
#endif // TASKMANAGERINTERFACE_H