MultiHandOverAlgos.h

#ifndef MULTIHANDOVERALGOS_H
#define MULTIHANDOVERALGOS_H
#include <sstream>
#include "MultiAgentHandOvers.h"
#include "MultiHandOverUtils.hpp"

namespace HRICS {
namespace MHO{

class AbstractSolutionTools{
public:
    AbstractSolutionTools(void){
        setLogger(&_dummy_logger);
    }
    virtual ~AbstractSolutionTools(){}

    virtual double computeCost(MultiHandOverSolution* solution) = 0;
    virtual double computeNavigationTrajectories(MultiHandOverSolution* solution, int action_index) =0;
    //virtual bool handOverFindConfs(MultiHandOverSolution * solution, int frontier_index) =0;
    //virtual bool handOverFindConfs(MultiAgentCell *c1,unsigned int a1,MultiAgentCell* c2,unsigned int a2) =0;
    //virtual bool handOverFindConfs(FrontierPtr frontier) =0;
    virtual double distanceCost(double d,unsigned int a) =0;
    virtual double distanceCostNoTime(double d,unsigned int a) =0;
    virtual double distanceTime(double d, unsigned int a)=0;
    virtual double timeCost(double t,unsigned int a)=0;
    virtual double objectTimeCost(double t)=0;
    virtual double handOverCostEstimation(unsigned int i,unsigned int j)=0;
    virtual double handOverCost(FrontierPtr frontier,double *time=0) =0;
    virtual double handOverCost(MultiAgentCell *c1,unsigned int a1,MultiAgentCell *c2,unsigned int a2,double *time=0) =0;
    virtual double handOverMaxDist(unsigned int a1,unsigned int a2) =0;
    virtual double handOverSideCosts(MultiAgentCell *c1,unsigned int a1,MultiAgentCell* c2,unsigned int a2, double *coming_time=0) =0;
    void setLogger(AlgoLoggerInterface* logger){_logger=logger;}
private:
    AlgoLoggerInterface *_logger;
    AlgoLoggerInterface _dummy_logger;
};


class AbstractFrontierOptimizer{
public:
    AbstractFrontierOptimizer(){
        setLogger(&_dummy_logger);
    }
    virtual ~AbstractFrontierOptimizer(){}

    virtual MultiHandOverSolution* optimize(MultiHandOverSolution * solution, unsigned int action_index,double keep_dist,double min_dist) =0;

    void setLogger(AlgoLoggerInterface* logger){_logger=logger;}
protected:
    AlgoLoggerInterface *_logger;
private:
    AlgoLoggerInterface _dummy_logger;
};


class ToolSet{
public:
    ToolSet(FrontierTester *frontier_tester, AbstractSolutionTools *sol_tools, MultiHandOver *mho, FrontierCells::FrontierCheckStatus max_check_status);
    AbstracHandOverObjectCheck *handoverObjectChecker;
    AbstractSolutionTools *solutionTools;
    FrontierTester *frontierTester;

    void setRobots(unsigned int a1,unsigned int a2);
    void updateObject();
    bool checkHandOverFull(MultiAgentCell *c1,unsigned int a1,MultiAgentCell* c2,unsigned int a2,double &cost,double *time=0);
    bool checkFrontierFull(FrontierPtr frontier,MultiAgentCell *c1,unsigned int a1,MultiAgentCell *c2, unsigned int a2);
    double getReach(unsigned int a);
    double getReach(unsigned int a1,unsigned int a2);

    static double distanceCostStatic(void *the_this,double d,unsigned int agent){return ((ToolSet*)the_this)->solutionTools->distanceCost(d,agent);}

    double getStatsCheckHoFull(double &mean, double &variance, unsigned int &n);

private:
    MultiHandOver *_mho;
    FrontierCells::FrontierCheckStatus _max_check_status;

    //STAT
    unsigned int _full_ho_cnt;
    double _full_ho_time_mean;
    double _full_ho_time_M2; //for online variance computation

};

class SolutionTools : public AbstractSolutionTools{
    MOVE3D_STATIC_LOGGER;
public:
    SolutionTools(MultiHandOver *mho);
    double computeCost(MultiHandOverSolution *solution);
    double computeNavigationTrajectories(MultiHandOverSolution* solution, int action_index=-1);
    //bool handOverFindConfs(MultiHandOverSolution * solution, int frontier_index=-1);
    //bool handOverFindConfs(MultiAgentCell *c1,unsigned int a1,MultiAgentCell* c2,unsigned int a2);
    //bool handOverFindConfs(FrontierPtr frontier);
    double distanceCostNoTime(double d,unsigned int a);
    double distanceCost(double d,unsigned int a);
    double distanceTime(double d, unsigned int a);
    double timeCost(double t, unsigned int a);
    double objectTimeCost(double t);
    double handOverCostEstimation(unsigned int i, unsigned int j);
    double handOverCost(FrontierPtr frontier,double *time=0);
    double handOverCost(MultiAgentCell *c1,unsigned int a1,MultiAgentCell *c2,unsigned int a2,double *time=0);
    double handOverMaxDist(unsigned int a1, unsigned int a2);
    double handOverSideCosts(MultiAgentCell *c1, unsigned int a1, MultiAgentCell* c2, unsigned int a2, double *coming_time=0);
private:
    MultiHandOver *_mho;
};

class FrontierOptimizerClimber : public AbstractFrontierOptimizer{
public:
    //FrontierOptimizerClimber(MultiAgentGrid* grid, AbstractSolutionTools* solution_cost_computer,AbstracHandOverObjectCheck *ho_check);
    FrontierOptimizerClimber(MultiAgentGrid* grid, ToolSet *tool_set);
    virtual ~FrontierOptimizerClimber(){}
    MultiHandOverSolution* optimize(MultiHandOverSolution *solution, unsigned int action_index,double keep_distance,double min_distance);
    MultiAgentCell* findOptimalCellNullDistanceFrontier(MultiHandOverSolution *solution, unsigned int action_index);
private:
    MultiAgentGrid* _grid;
    ToolSet *_tool_set;
    //AbstractSolutionTools* _solution_tools;
    //AbstracHandOverObjectCheck* _ho_checker;
    bool _showText;
};

class HandOverDirectChecker : public AbstracHandOverObjectCheck{
public:
    HandOverDirectChecker(Robot* object, Robot *r1=0, Robot *r2=0);
    virtual ~HandOverDirectChecker(){}
    bool check();
    confPtr_t getQMiddle(){return q_middle;}
    virtual std::string getName(){return _name;}
private:
    void setOrientationObjectForHO(Eigen::Vector3d & dir1);
    Configuration confForBBAxisAlign(Eigen::Vector3d const & main_dir, Eigen::Vector3d const & second_dir);
    confPtr_t q_middle;
    std::vector<Eigen::Vector3d> _object_bb_vects;
    std::vector<int> _object_directions_sorted_for_ho;

    static const std::string _name;

};

class HandOverWindowCheckerRRT : public AbstracHandOverObjectCheck{
public:
    HandOverWindowCheckerRRT(Robot *obj, Robot *r1=0, Robot *r2=0, bool dummy_smooth=true);
    HandOverWindowCheckerRRT(Robot *obj, Robot *r1, Robot *r2, bool dummy_smooth,Eigen::Vector3d _shoulder1, Eigen::Vector3d _shoulder2, double reach1, double reach2);
    virtual ~HandOverWindowCheckerRRT(){}
    bool check();
    void setUseDummySmooth(bool dummy){_use_dummy_smooth=dummy;}
    confPtr_t getQMiddle(){return q_middle;}
    virtual std::string getName(){return _name;}
private:
    void constructBB();
    void setObjectLimits();
    void resetObjectLimits();
    void saveObjectLimits();

    bool computeRRT(double time_limit_ms);
    static inline Eigen::Vector2d v3dto2d(Eigen::Vector3d v3);

    vector<double> object_limits_bkp;

    std::pair<Eigen::Vector3d,Eigen::Vector3d> bb;
    bool _use_dummy_smooth;

    confPtr_t q_middle;

    static const std::string _name;
};

}
}
#endif // MULTIHANDOVERALGOS_H