MultiHandOverUtils.hpp

#ifndef MULTIHANDOVERUTILS_HPP
#define MULTIHANDOVERUTILS_HPP

#include <Eigen/Core>

#include "P3d-pkg.h"
#include "API/Device/robot.hpp"
#include "HRI_costspace/Grid/HRICS_MultiAgentGrid.hpp"
#include "multiHandOver/LazyWAstar.h"
#include "multiHandOver/SimpleGraph.h"
//#include "MultiHandOverAlgos.h"
//#include "HRI_costspace/HRICS_MultiHandOver.h"
#include "utils/multiHandOver/graphAlgorithms.h"
#include "Logging/Logger.h"

#include "GTP/taskManagerInterface.hpp"


extern taskManagerInterface *TMI;


namespace HRICS {
class MultiHandOver;

namespace MHO{
class AbstractSolutionTools;
class AbstracHandOverObjectCheck;
class ToolSet;


class AlgoLoggerInterface{
public:
    AlgoLoggerInterface()
    {
    }
    virtual ~AlgoLoggerInterface(){}
    //virtual void textlog(std::stringstream ss){}
    virtual void drawcells(MultiAgentCell* c1,MultiAgentCell* c2,bool marker){}
    virtual void draw() {}
    virtual void resetdisp() {}
    //virtual void drawcellsdist(std::vector<MultiAgentCell*> &cells) =0;
};

class ConfigFileParser
{
public:
    ConfigFileParser(MultiHandOver *mho);
    bool configure_from_xml(std::istream & is);
private:
    MultiHandOver* _mho;
};

class MultiHandOverAgent{
public:
    MultiHandOverAgent(Robot *r,double use_cost,double speed,unsigned int index);
    Robot *robot;
    bool is_human;
    unsigned int index;
    std::string shortname;
    double use_cost;
    double speed;
    double safe_dist;
    double handover_comfort_dist;
    double reach();
    double shoulderHeight();
    bool have_final_position;
};

class MultiHandOver3dPlanner
{
public:
    MultiHandOver3dPlanner(Robot * robot, bool is_human);

    bool grabConfIK(configPt &q, confPtr_t robotPos, const Eigen::Vector3d & grab_point);

    bool grabConfCheck(configPt &q, const Eigen::Vector2d robot_pos, const Eigen::Vector3d & grab_point);
    bool grabConfIK_human(configPt &q, confPtr_t const humanPos, Eigen::Vector3d const & grab_point);

    bool computeHandOverTrajectory(confPtr_t q_start,confPtr_t q_end);
    void clearTraj(confPtr_t conf=confPtr_t((Configuration*)0));
    bool computeTrajectory(confPtr_t start_conf,confPtr_t nav_conf, std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > const& path2d, confPtr_t end_conf);
    bool computePath2dToPath3dShortCut(std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > const& path2d, std::vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> > &path3d );

private:
    Robot * _robot;
    bool _is_human;
};



class MultiHandOverSolution{
public:
    class Action{
    private:
        //Robot *agentA,*agentB;
        //FrontierPtr frontier;
        //std::vector<MultiAgentCell*> &pathA, &pathB;
        //double &distA,&distB;
        unsigned int _f;
        MultiHandOverSolution * _s;
        static void checkI(unsigned int i){if(i>1){throw(std::invalid_argument("argument must be 0 or 1"));}}
    public:
        Action(unsigned int frontier_index,MultiHandOverSolution * solution);
        Robot* robot(unsigned int i){checkI(i);return _s->agents[_f+i]->robot;}
        MultiHandOverAgent * agent(unsigned int i){checkI(i);return _s->agents[_f+i];}
        std::vector<MultiAgentCell*> & path(unsigned int i){ checkI(i); return _s->navigation_trajs[_f*2+i];}
        void setPath(unsigned int i,std::vector<MultiAgentCell*> &path){ checkI(i); _s->navigation_trajs[_f*2+i]=path;}
        MultiAgentCell* from(unsigned int i){return (i<2 && _s->navigation_trajs[_f*2+i].size() ? _s->navigation_trajs[_f*2+i][0] : 0);}
        MultiAgentCell* to(unsigned int i){return (i<2 && _s->navigation_trajs[_f*2+i].size() ? _s->navigation_trajs[_f*2+i].back() : 0);}
        double dist(unsigned int i){return (i<2 ? _s->nav_dists[_f*2+i]: -1 );}
        void setDist(unsigned int i,double d){(i<2 ? _s->nav_dists[_f*2+i]=d: -1 );}
        double startDate(unsigned int i){checkI(i); return _s->nav_start_dates[_f*2+i];}
        void setStartDate(unsigned int i, double date_s){checkI(i); _s->nav_start_dates[_f*2+i]=date_s;}
        double startDateHO(){return _s->ho_start_end_dates[_f].first;}
        void setStartDateHO(double start_s){_s->ho_start_end_dates[_f].first=start_s;}
        double endDateHO(){return _s->ho_start_end_dates[_f].second;}
        void setEndDateHO(double end_s){_s->ho_start_end_dates[_f].second=end_s;}
        double navTime(unsigned int i){checkI(i); return _s->nav_times[_f*2+i];}
        void setNavTime(unsigned int i, double time_s){checkI(i); _s->nav_times[_f*2+i] = time_s;}
        Configuration & initQ(unsigned int i){checkI(i); return _s->initial_confs[_f+i];}
        FrontierPtr frontier(){return _s->frontiers[_f];}
        MultiHandOverSolution* getSolution(void){return _s;}
        confPtr_t objectPos();
        confPtr_t agentConf(unsigned int i);
    };
    struct CostData{
        CostData():dist(-1),time(-1),use_cost(-1),handover_cost(-1),handovers(0),marker_is_opt(0),agent_names(0){}
        CostData(MultiHandOverSolution* sol);
        double dist;
        double time;
        double use_cost;
        double handover_cost;
        unsigned int handovers;
        unsigned int go_to_target_nb;
        bool marker_is_opt;
        std::vector<std::string> agent_names;
    };

    MultiHandOverSolution(void);
    MultiHandOverSolution *cloneAsSon(void);
    MultiHandOverSolution *clone(void);
    unsigned int getId(void){return _id;}
    bool addHandOverAction(FrontierPtr frontier);

    double getTotalDist();
    double getTotalUseCost();

    double cost;
    double time;
    double cumul_time_nav;
    double hri_cost;
    std::vector<Configuration> initial_confs,target_confs;
    //trajectories traj; ///< the complete solution as trajectories for each agents
    std::vector<MultiHandOverAgent*> agents; 
    std::vector<Action> actions;
    std::vector<std::tr1::shared_ptr<FrontierCells> > frontiers; 

    std::vector<double> avail_delay;   
    std::vector<double> nav_start_dates;
    std::vector<std::pair<double,double> > ho_start_end_dates;
    std::vector<double> handover_times; 
    std::vector<double> handover_hri_costs; 
    std::vector<std::vector<MultiAgentCell*> > navigation_trajs;
    std::vector<double> nav_dists;
    std::vector<double> nav_times;
    std::vector<std::vector<MultiAgentCell*> > navigation_trajs_to_target;
    std::vector<double> nav_dists_to_target;
    std::vector<std::pair<double,double> > nav_start_end_times_to_target;
    bool is_optimized;
    friend std::ostream& operator<<(std::ostream& os, const MultiHandOverSolution& sol);
    friend bool operator<(const MultiHandOverSolution &left,const MultiHandOverSolution &right);
    friend bool operator>(const MultiHandOverSolution &left,const MultiHandOverSolution &right);
    static bool less(MultiHandOverSolution const *left,MultiHandOverSolution const *right);
    std::string print_long();

    //solution modification graph
    std::vector<MultiHandOverSolution*> sons;
    MultiHandOverSolution * parent;
    MultiHandOverSolution * oldest_parent;

    Robot* object;

    static void printFamiliesTrees(std::vector<MultiHandOverSolution*> &solutions, std::ofstream &output);
    static void resetID(void){MultiHandOverSolution::_current_id=0;}
private:
    static unsigned int _current_id;
    unsigned int _id;
};


class SubTask{
public:
    typedef enum {UNDEF=0, HO_GIVE, HO_GET, NAV_WITH_OBJ, NAV_TO_HO, NAV_TO_TARGET, NAV_SYNC, NAV_OTHER,IDLE} TaskType_t;
    typedef enum {SHO_NONE=0,     
                  SHO_G_GIVE,       
                  SHO_R_PICK,       
                  SHO_G_RELEASE,    
                  SHO_R_GRAB,       
                  SHO_G_DISENGAGE,  
                  SHO_R_TAKE,       
                  SHO_ENUM_SIZE} HandoverStepType_t;
    typedef enum {IS_2D=0, HAS_FULL_TRAJ, COMPLETED, RUNNING, SIZE} MarkerList_t;
    SubTask();
    SubTask(MultiHandOverAgent* agent, TaskType_t type, double beginDate, double endDate, double cost=0, HandoverStepType_t hostepType = SHO_NONE);
    virtual ~SubTask();

    void resetMarker();
    bool getMarker(MarkerList_t m);
    void setMarker(MarkerList_t m, bool b);

    TaskType_t type() const ;
    HandoverStepType_t hoStepType() const;
    bool isNav()     const ;
    bool isManip()   const ;

    double beginDate();
    double endDate();
    double duration();
    void extendToEndDate(double new_end_date);
    void extendDuration(double new_duration);
    void replan(double new_begin);
    double cost();

    MultiHandOverAgent *agent();

    std::vector<confPtr_t> & waypoints();
    std::vector<Eigen::Vector2d> & waypoints2d();
    void addWaypoint(confPtr_t q);
    void addWaypoint(Eigen::Vector2d v, double date=-numeric_limits<double>::infinity());
    std::vector<double> & waypointDates();
    API::Trajectory &traj();
    void traj(API::Trajectory traj);
    void spreadIdlePositionToNext();

    void addPrevious(SubTask* prev);
    void addNext(SubTask* next);
    std::vector<SubTask*> & getPrev(){return this->prev;}
    std::vector<SubTask*> & getNext(){return this->next;}
    void substitutePrev(SubTask *prev,SubTask *new_prev);
    void substituteNext(SubTask *next,SubTask *new_next);

    unsigned int getId(){return this->_id;}
    static bool compareLessBeginDate(SubTask &t1,SubTask &t2){return t1.beginDate() < t2.beginDate();}
    static bool compareLessBeginDatePtr(SubTask *t1,SubTask *t2){return t1->beginDate() < t2->beginDate();}

    static std::string typeString(SubTask::TaskType_t t);

    //TODO for debug:
    std::string comment;
private:
    TaskType_t _type;
    HandoverStepType_t _handoverStepType;
    double _begin_date,_end_date;
    double _duration;
    double _cost;
    MultiHandOverAgent *_agent;
    std::vector<confPtr_t> _wp;
    std::vector<Eigen::Vector2d> _2d_wp;
    std::vector<double> _dates_wp;
    API::Trajectory _traj;

    std::vector<SubTask*> prev;
    std::vector<SubTask*> next;

    std::vector<bool> _markers;

    unsigned int _id;
    static unsigned int _count_id;
};

class Task{
public:
    class state_iterator
    {
    public:
        state_iterator(Task* task);
        bool advance();
        std::map<MHO::MultiHandOverAgent*,Eigen::Vector2d> & state();
        SubTask* getSubTask(MultiHandOverAgent* agent);
        double date();
        bool isEnd();

    private:
        Task* _task;
        bool _is_end_iterator;
        HRICS::MultiAgentGrid* _grid;
        std::map<MHO::MultiHandOverAgent*,Eigen::Vector2d>  _state_current;
        double _time;
        std::map<MHO::SubTask*,pair<int, double> > _trak; 
        std::map<MHO::SubTask*,pair<int, double> > _next; 
        std::map<MHO::MultiHandOverAgent*,MHO::SubTask*> _sub_task;
    };

    Task(MultiHandOverSolution* sol);
    ~Task();
    SubTask* getSubTask(int i);
    std::vector<HRICS::MHO::MultiHandOverAgent*> const & getAgents();
    std::vector<SubTask *> const & getSubTasks();
    void addSubTask(SubTask* st, bool no_sort=false);
    void eraseSubTask(SubTask* st);
    void plotGanttChart();
    void displayGraph();
    void removeIdles();
    void fillWithIdle(map<MultiHandOverAgent *, MultiAgentCell *> &state_begin);
    state_iterator begin();

    void refineHandOvers();
    void refineHandOver(SubTask *st1,SubTask *st2);

    void setObject(Robot* obj){_object=obj;}
    Robot *getObject(){return _object;}

private:
    std::vector<SubTask*> _sub_tasks;
    vector<MultiHandOverAgent*>  _agents;
    Robot* _object;
};

struct SubTaskMHP{
    //SubTaskMHP(SubTask *st);
    std::string robotname;
    std::vector<int> nexts;
    int index;
    double begin,end;
    std::vector<confPtr_t> wp;
    SubTask::TaskType_t type;
};

struct MultiHandOverMhpHO
{
    MultiHandOverMhpHO(MultiHandOverSolution *sol,unsigned int step);
    std::string g_name; 
    Eigen::Vector3d g_pose;
    enum {ARM_LEFT,ARM_RIGHT,ARM_UNDEF} g_arm;
    std::vector<double> g_arm_conf;
    double g_torso;

    std::string r_name; 
    Eigen::Vector3d r_pose;
    double r_torso;
    std::vector<double> r_arm_conf;

    Eigen::Vector3d obj_pose;

    double r_start_time;
    double g_start_time;

};

class MultiHandOverHO
{
public:
    MultiHandOverHO(MultiHandOverSolution::Action *action, MultiHandOver* mho);
    MultiHandOverHO(MultiHandOverAgent* a1,MultiHandOverAgent *a2,MultiAgentCell* c1,MultiAgentCell* c2, MultiHandOver* mho);
    MultiHandOverHO(MultiHandOverAgent *a1, MultiHandOverAgent *a2, double d, MultiHandOver* mho);
    double computeCost();
    double humanRobotDistanceRelatedCost();

    //getters / setters
    double getCost(void) const;
    void setCost(double cost);
    double getTime(void) const;
    void setTime(double time_sec);

    Robot* robot(unsigned int i);
    MultiHandOverAgent* agent(unsigned int i);
    //FrontierPtr frontier(void);
    MultiAgentCell *cell(unsigned int i);

    //methods to find a handover
    static MultiHandOverHO handoverBetween2pos(MultiAgentCell* cellA,MultiAgentCell* cellB,unsigned int agentA, unsigned int agentB);
    static void getOptimalCost(MultiHandOverAgent *a1, MultiHandOverAgent *a2,double &cost,double &duration,MultiHandOver* mho);

private:
    double cost;
    double time;
    MultiHandOverAgent *_a1,*_a2;
    MultiAgentCell *_c1,*_c2;
    double _d;
    MultiHandOver* _mho;
    //MultiHandOverSolution::Action *_action;

};

class HandOverFinder
{
public:
    HandOverFinder(MultiHandOver* mho,MultiAgentGrid *grid,unsigned int agent1,unsigned int agent2,
                   MultiAgentCell* cell1_O,MultiAgentCell* cell2_O,MultiAgentCell* cell1_T,MultiAgentCell* cell2_T);
    FrontierPtr findFrom2Points();
    //void setCostFunction(double (*func)(double,unsigned int)){_cost_function=func;}

private:
    MultiAgentGrid *_grid;
    MultiHandOver *_mho;
    unsigned int _agent1,_agent2;
    MultiAgentCell *_cell1_ori,*_cell2_ori,*_cell1_dest,*_cell2_dest; 

    //double (*_cost_function)(double ,unsigned int);
};

class MultiHandOverHeuristic{
public:
    MultiHandOverHeuristic(MultiHandOver *mho, MHO::AbstractSolutionTools *s_tools);
    virtual ~MultiHandOverHeuristic(){}
    virtual double compute(MultiAgentCell* c1,unsigned int a1,MultiAgentCell* c2,unsigned int a2);
    std::vector<double> getTimes(){return _times;}
protected:
    void doTheGraph();
    mho::ReverseDisjkstra& getRevDijkstra(){return rev_dijk;}
    MultiHandOver* getMho(){return _mho;}
    MHO::AbstractSolutionTools* getSolutionTools(){return _tools;}
    mho::Node* getAgentNode(unsigned int i){return _agents_nodes[i];}
private:
    MultiHandOver *_mho;
    mho::SimpleGraph _graph;
    std::vector<mho::Node*> _agents_nodes;
    MHO::AbstractSolutionTools *_tools;
    //TODO remove related
    std::vector<double> _times;
    mho::ReverseDisjkstra rev_dijk;
};

class MultiHandOverHeuristicClosestFrontier: public MultiHandOverHeuristic{
public:
    MultiHandOverHeuristicClosestFrontier(MultiHandOver *mho,MHO::AbstractSolutionTools *s_tools);

    virtual ~MultiHandOverHeuristicClosestFrontier(){}
    virtual double compute(MultiAgentCell *c1, unsigned int a1, MultiAgentCell *c2, unsigned int a2);

protected:
    virtual double findMinimalFrontier(MultiAgentCell* c1i,MultiAgentCell* c1g, unsigned int a1,
                                            MultiAgentCell *c2i,MultiAgentCell* c2g, unsigned int a2);
    Eigen::Vector2d findMinimalFrontier2pt(Eigen::Vector2d c1, double f1, Eigen::Vector2d c2, double f2);
    Eigen::Vector2d findMinimalFrontier3pt(Eigen::Vector2d ci,Eigen::Vector2d cg, double f1, Eigen::Vector2d c2, double f2);
    Eigen::Vector2d findMinimalFrontier4pt(Eigen::Vector2d c1i,Eigen::Vector2d c1g, double f1, Eigen::Vector2d c2i, Eigen::Vector2d c2g, double f2);

};

class LazyWeightedAstarNode : public mho::LWANodeBase{
    MOVE3D_STATIC_LOGGER;
public:
    LazyWeightedAstarNode(MultiAgentCell* cell, unsigned int agent,MultiHandOverHeuristic *heuristic, ToolSet *tool_set);
    LazyWeightedAstarNode(LazyWeightedAstarNode *other);
    virtual ~LazyWeightedAstarNode();
    virtual LWANodeInterface *clone();

    std::vector<LWANodeInterface*> sons();

    virtual void g(double g);
    virtual double computeEndDate();
    virtual bool isAgentAlreadyUsed();
    virtual double edgeCostEstimationFrom(LWANodeInterface *parent);
    virtual double computeHeuristic(LWANodeInterface *goal);
    virtual double computeTrueCost(LWANodeInterface *parent);
    virtual bool isSameConf(LWANodeInterface *other);

    static LazyWeightedAstarNode *cast(LWANodeInterface* p){return dynamic_cast<LazyWeightedAstarNode*>(p);}

    MultiAgentCell *cell(){return _cell;}
    void cell(MultiAgentCell* c){_cell=c;}
    unsigned int agent(){return _agent;}
    void agent(unsigned int a){_agent=a;}
    void parent(LazyWeightedAstarNode* p);
    LWANodeInterface* parent(){return mho::LWANodeBase::parent();}

private:
    MultiAgentCell *_cell;
    unsigned int _agent;
    MultiHandOverHeuristic *_heuristic_class;
    MHO::ToolSet *_toolset;
    std::vector<bool> _agents_in_parents;
    double _time,_last_computed_time,_last_computed_cost;


};

class DisplayLWAstarInGrid : public mho::DisplaySearchAstarInterface
{
public:
    DisplayLWAstarInGrid(MultiAgentGrid *grid, bool showtext, bool draw=true);
    virtual ~DisplayLWAstarInGrid(){}
    void onOpen(mho::LWANodeInterface* open);
    void onClose(mho::LWANodeInterface* closed);
    void onFound(mho::LWANodeInterface* goal);
    void onEnd(std::vector<pair<unsigned int,double> > times);
    void setDraw(bool b){_draw=b;}
    std::vector<std::pair<unsigned int,double> > getTimes();

private:
    MultiAgentGrid *_grid;
    bool _marker_is_first_call;
    bool _showText,_draw;
    std::vector<MultiAgentCell*> _cells;
    double _max_draw_f;
    double count_open,count_closed;
    std::vector<std::pair<unsigned int,double> > _times;
};

}
}
#endif // MULTIHANDOVERUTILS_HPP