HRICS_MultiAgentGrid.hpp

#ifndef HRICS_MULTI_AGENT_GRID_HPP
#define HRICS_MULTI_AGENT_GRID_HPP
//#define DRAW_FRONTIER_HAND_IN_HAND
//#define DRAW_FRONTIER_STEPS // for generating pictures for algo explanation
//#define CARACT_TIME_FIND_CELLS_IN_RANGE
#define DRAW_OPTIMIZE_FRONTIER

#include "API/planningAPI.hpp"
#include "API/Grids/gridsAPI.hpp"
#include "Logging/Logger.h"

namespace HRICS
{

class MetaFrontierCells;
struct FrontierCells;
class MultiAgentCell;

typedef std::tr1::shared_ptr<FrontierCells> FrontierPtr;
typedef std::tr1::shared_ptr<MetaFrontierCells> MetaFrontierPtr;


class MultiAgentGrid : public API::TwoDGrid
{
    MOVE3D_STATIC_LOGGER;
public:
    class AgentInGrid;
    class MetaAgentInGrid{
    public:
        typedef std::tr1::shared_ptr<MetaAgentInGrid> s_ptr;
        MetaAgentInGrid(unsigned int agent_number);
        MetaAgentInGrid::s_ptr shared_clone();
        std::vector<AgentInGrid*> & getAgentsInGrid(void){return this->_realAgents;}
        void addAgent(AgentInGrid* a,s_ptr s_ptr_this);
    private:
        void removeAgent(AgentInGrid* a);
    public:
        //getter/setters
        Robot* getCylinder(void){return _cylinder;}
        double getCylinderRadius(void){return _cyl_radius;}
        void setCylinder(Robot * c);
        std::vector<MultiAgentCell*> & reachable(){return _reachable;}
        std::vector<MultiAgentCell*> & boundaries(){return _boundaries;}
        std::vector<MultiAgentCell* > & frontiers(MetaAgentInGrid::s_ptr other){return _frontiers[other.get()];}
        std::vector<MultiAgentCell* > & frontiers_ok(MetaAgentInGrid::s_ptr other){return _frontiers_ok[other.get()];}
        bool frontierFullyChecked(MetaAgentInGrid::s_ptr other){return _frontierFullyChecked[other.get()];}
        void setFrontierFullyChecked(MetaAgentInGrid::s_ptr other,bool b=true){_frontierFullyChecked[other.get()]=b;}
        bool isFrontierComputed(MetaAgentInGrid::s_ptr other){return _frontiersComputed[other.get()];}
        void setFrontierComputed(MetaAgentInGrid::s_ptr other,bool b=true){_frontiersComputed[other.get()]=b;}

        char letter_id;
    private:
        Robot* _cylinder; 
        double _cyl_radius; 
        std::vector<MultiAgentCell*> _reachable;
        std::vector<MultiAgentCell*> _boundaries;
        std::map<MetaAgentInGrid*, std::vector<MultiAgentCell* > >  _frontiers;
        std::map<MetaAgentInGrid*, std::vector<MultiAgentCell* > >  _frontiers_ok;
        std::map<MetaAgentInGrid*, bool> _frontierFullyChecked;
        std::map<MetaAgentInGrid*, bool> _frontiersComputed;

        std::vector<AgentInGrid*> _realAgents;
        static char _letter_id_cnt;
    };

    class AgentInGrid{
    public:
        Robot* getRobot(){return _robot;} 
        void setRobot(Robot* r){_robot=r;}
        Robot* getCylinder(){return _metaAgent->getCylinder();} 
        double getMaxDist(){return _maxDist;} 
        void setMaxDist(double d){_maxDist=d;}
        //std::vector<MultiAgentCell*> accessible; ///<the list of cells where the agent can 'fit'. NOT where it actually can travel. @warning obsolete
        MultiAgentCell* getOrigin(){return this->_origin;}
        void setOrigin(MultiAgentCell* c){this->_origin=c;}
        MultiAgentCell* getTarget(){return this->_target;}
        void setTarget(MultiAgentCell* c){this->_target=c;}
        std::vector<MultiAgentCell*> & reachable(){return _metaAgent->reachable();}
        std::vector<MultiAgentCell*> & boundaries(){return _metaAgent->boundaries();}
        std::vector<MultiAgentCell* > & frontiers(AgentInGrid *other){return getMetaAgent()->frontiers(other->getMetaAgent());}
        bool frontierFullyChecked(AgentInGrid* other){return _metaAgent->frontierFullyChecked(other->getMetaAgent());}
        void setFrontierFullyChecked(AgentInGrid* other,bool b=true){_metaAgent->setFrontierFullyChecked(other->getMetaAgent(),b);}
        bool isHuman(){return getRobot()->getHriAgent()->is_human;} 
        double getSpeed(void){return _speed;} 
        void setSpeed(double s){_speed=s;}
        bool isDistanceComputed(){return _distanceComputed;} 
        void setDistanceComputed(bool b=true){_distanceComputed=b;}
        void setMetaAgent(MetaAgentInGrid::s_ptr meta){_metaAgent=meta;}
        MetaAgentInGrid::s_ptr getMetaAgent(void){return _metaAgent;}
    private:
        Robot* _robot;
        double _speed;
        double _maxDist;
        bool _distanceComputed;
        MetaAgentInGrid::s_ptr _metaAgent;
        MultiAgentCell *_origin,*_target;

    };

    struct ComputationStat{
        timespec access_total_time;
        std::vector<timespec> reachability_time;
        timespec reachability_total_time;
        std::vector<timespec> frontiers_time;
        std::vector<unsigned int> frontiers_iterations;
        timespec frontiers_total_time;
        unsigned int frontiers_total_iterations;
        ComputationStat(void){
            access_total_time.tv_nsec = 0;
            access_total_time.tv_sec = 0;
            reachability_time.clear();
            reachability_total_time.tv_nsec=0;
            reachability_total_time.tv_sec=0;
            reset_frontiers(0);
        }
        void reset_frontiers(unsigned int size){
            frontiers_time.assign(size,null_ts());
            frontiers_iterations.assign(size,0);
            frontiers_total_iterations=0;
            frontiers_total_time=null_ts();
        }
        static timespec null_ts(void){timespec nts;nts.tv_nsec=nts.tv_sec=0;return nts;}
    };

    MultiAgentGrid();
    MultiAgentGrid(double pace, std::vector<double> envSize);
    MultiAgentGrid(double pace, std::vector<double> envSize, std::vector<Robot *> &agents);
    ~MultiAgentGrid();

    void resetData(void);
    //getters and setters
    void setAgents(std::vector<Robot*> &agents);
    std::vector<Robot*> & getAgents() { return mAgents; }
    AgentInGrid* getAgentStructAt(unsigned int index){return mAgentsInGrid[index];}

    void setAgentTarget(unsigned int a);

    double getNbCellX() {return _nbCellsX;}
    double getNbCellY() {return _nbCellsY;}

    void setAsNotSorted() {gridIsSorted = false;}

    void dumpVar();

    void init(void);
    void initCylinders(void);
    void computeAccessibility(void);
    void computeAccessAndDistCombined(void);

    std::vector<MultiAgentCell*> computeOtherTrajectory(MultiAgentCell * cell_origin, MultiAgentCell *cell_dest, unsigned int agent_index,double epsilon=1,double dist_max=-1,int max_expansions=-1);

    bool areCellsInNeighbourhood(MultiAgentCell* c1,MultiAgentCell *c2,unsigned int a,double dist);

    API::TwoDCell* createNewCell(unsigned int index,unsigned  int x,unsigned  int y );

    void computeDistances();
    void computeDistancesForAgent(MultiAgentCell* cell, unsigned int agent_index);
    void computeAltDistancesForAgent(MultiAgentCell* origin, unsigned int agent_index);
    void enableAltDistancesSums(bool b=true){_enable_alt_distances_sums=b;if(b){this->clearAltDistancesInCells();}}
    void clearAltDistancesInCells(void);

    unsigned int getMaxAlternativeDistancesForCells(void){return _max_alternative_distances_for_cells;}
    void getMaxAlternativeDistancesForCells(unsigned int v){_max_alternative_distances_for_cells=v;}


    std::pair<MultiAgentCell*,MultiAgentCell*> neighborSolution(unsigned int agent_1,unsigned int agent_2,std::pair<MultiAgentCell*,MultiAgentCell*> solution);
    void searchAllFrontiers();
    std::vector<FrontierPtr > searchFrontiers(unsigned int agent_1,unsigned int agent_2,double keep_distance);
    std::vector<FrontierPtr > simulatedAnnealingFrontierSearch(unsigned int agent_1,unsigned int agent_2,double keep_distance);
    std::vector<MetaFrontierPtr > hillClimberFrontierSearch(unsigned int agent1,unsigned int agent2, double keep_distance);
    std::vector<FrontierPtr > frontierSearch(unsigned int agent1,unsigned int agent2, double keep_distance);
    std::vector<FrontierPtr > exhaustiveFrontierSearch(unsigned int agent1,unsigned int agent2, double keep_distance);
    std::vector<FrontierPtr > handInHandFrontierSearch(unsigned int agent1,unsigned int agent2, double keep_distance);

    void saveFrontiers(std::ofstream & output);
    bool loadFrontiers(std::ifstream & input);

    void initVectorFrontiers(void);
    unsigned int getFrontierIndex(unsigned int i,unsigned int j);
    unsigned int getFrontierIndex(std::pair<unsigned int,unsigned int> ind){return getFrontierIndex(ind.first,ind.second);}
    std::pair<unsigned int,unsigned int> getFrontierAgentIndices(unsigned int i);
    //std::vector<std::tr1::shared_ptr<FrontierCells> > & getFrontier(unsigned int i,unsigned int j);
    std::vector<MultiAgentCell*> const & getFrontierCells(unsigned int i, unsigned int j);
    //void setFrontier(unsigned int i,unsigned int j,vector<std::tr1::shared_ptr<FrontierCells> > & frontier);
    double getArmReach(unsigned int i);
    double getArmReach(AgentInGrid *a);
    double getHandOverReach(unsigned int i,unsigned int j);

    void addFrontier(unsigned int agent,unsigned int other, MultiAgentCell * cell);
    FrontierPtr getRandomFrontier(unsigned int a1,unsigned int a2, double timeout_ms=-1);
    std::vector<MultiAgentCell*> & getFrontierCheckedOk(unsigned int a1,unsigned int a2);

    double getFrontierSearchTempDecrease(void){return _frontier_search_temp_decrease;}
    void setFrontierSearchTempDecrease(double v){_frontier_search_temp_decrease=v;}
    double getFrontierSearchStartTemp(void){return _frontier_search_start_temp;}
    void setFrontierSearchStartTemp(double v){_frontier_search_start_temp=v;}
    unsigned int getFrontierSearchMaxIt(void){return _frontier_search_max_it;}
    void setFrontierSearchMaxIt(unsigned int v){_frontier_search_max_it=v;}
    double getFrontierSearchStopDist(void){return _frontier_search_stop_dist;}
    void setFrontierSearchStopDist(double v){_frontier_search_stop_dist=(v==0 ? std::min(this->_cellSize[0],this->_cellSize[1]): v);}
    ComputationStat & getComputationStats(void){return this->_computation_stat;}


    void draw();
private:
    void _drawCell(MultiAgentCell *cell, double *color);

public:
    void setAgentIndexToDrawGrid(int index){
        this->_agent_index_to_draw_grid=index;
    }
    void setDrawAgentAccessibilityGrid(bool b,bool boundariesOnly=false){this->_drawAgentReachGrid=b; this->_drawBoundariesOnly=boundariesOnly;}
    void setDrawAStar(bool b){_draw_a_star=b;}
    void setDrawFrontiers(bool b,unsigned int i=0,unsigned int j=0){_drawFrontiers=b;_drawFrontierIndices.first=i;_drawFrontierIndices.second=j;}
    void setDrawFrontiersSearchLog(bool b){_drawLog=b;}
    unsigned int getFrontierSearchLogSize(void);
    void setFrontierSearchLogIndex(unsigned int i){_log_draw_window=i;}
    void setFrontierSearchLogWindow(unsigned int i){_log_draw_window_size=i;}

#ifdef DRAW_OPTIMIZE_FRONTIER
    void setDrawGridForOptim(bool b,std::vector<std::pair<MultiAgentCell*,unsigned int> > &cells_agents){_draw_grid_for_optim=b;_draw_grid_for_optim_cells_agents=cells_agents;}
    void unsetDrawGridForOptim(void){_draw_grid_for_optim=false;_draw_grid_for_optim_cells_agents.clear();}
#endif
#if defined(DRAW_FRONTIER_HAND_IN_HAND) || defined(DRAW_OPTIMIZE_FRONTIER)
    void setDrawOptimizeFrontier(MultiAgentCell *c1,MultiAgentCell* c2,bool ok){_draw_frontier_search_cells.first=c1;_draw_frontier_search_cells.second=c2;_draw_frontier_search_is_pair_ok=ok;_draw_frontier_search=true;}
    void unsetDrawOptimizeFrontier(void){_draw_frontier_search=false;}
#endif

    void setDrawExtraCells(std::vector<MultiAgentCell*>::iterator first,std::vector<MultiAgentCell*>::iterator last,double ratio_max){
        _draw_extra=true;
        _draw_extra_cells=std::make_pair(first,last);
        _temp_color_max_ratio=ratio_max;
    }
    void unsetDrawExtraCells(){_draw_extra=false;_draw_extra_cells.first=_draw_extra_cells.second;_temp_color_max_ratio=1;}


    void setCellsToblankCost();

    void initAllCellState();

    void initAllTrajs();

    void resetAllAccessibility();
    void resetDistances(void);

//    std::vector<std::pair<double,MultiAgentCell*> > getSortedGrid();

private:
    std::vector<Robot*> mAgents;
    std::vector<AgentInGrid*> mAgentsInGrid;

    Robot* robotCyl; 
    Robot* humCyl;

    std::vector<std::pair<double,MultiAgentCell*> > sortedGrid;

    bool gridIsSorted;

    unsigned int _max_alternative_distances_for_cells;
    bool _enable_alt_distances_sums;
    std::pair<MultiAgentCell*,double> _best_cell_in_alt_dist_sum;

    //frontier searching
    bool _use_reachable_for_frontier_search;
    //std::vector<std::vector<std::tr1::shared_ptr<FrontierCells> > > _frontiers;
    double _frontier_search_temp_decrease;
    double _frontier_search_start_temp;
    unsigned int _frontier_search_max_it;
    double _frontier_search_stop_dist;
    std::vector<std::vector<std::pair<double,std::pair<MultiAgentCell*,MultiAgentCell*> > > > _log_frontier_seeking;
    unsigned int _log_draw_window;
    unsigned int _log_draw_window_size;
    bool _save_log_frontier;


    //grid drawing parameters
    bool _drawAgentReachGrid;
    bool _drawBoundariesOnly;
    bool _draw_a_star;
#if defined(DRAW_FRONTIER_HAND_IN_HAND) || defined(DRAW_OPTIMIZE_FRONTIER)
    bool _draw_frontier_search;
    std::pair<MultiAgentCell*,MultiAgentCell*> _draw_frontier_search_cells;
    bool _draw_frontier_search_is_pair_ok;
#endif
#ifdef DRAW_OPTIMIZE_FRONTIER
    bool _draw_grid_for_optim;
    std::vector<std::pair<MultiAgentCell*,unsigned int> >  _draw_grid_for_optim_cells_agents;
#endif
#ifdef DRAW_FRONTIER_STEPS
    bool _draw_frontier_steps;
    std::vector<MultiAgentCell*> _draw_frontier_steps_cells;
#endif
    int _agent_index_to_draw_grid;
    bool _drawFrontiers;
    bool _drawLog; 
    std::pair<unsigned int, unsigned int> _drawFrontierIndices;
    ComputationStat _computation_stat;
    bool _draw_extra;
    std::pair<std::vector<MultiAgentCell*>::iterator,std::vector<MultiAgentCell*>::iterator > _draw_extra_cells;
    double _temp_color_max_ratio;


    //debug parameters
    bool _showText;

};

class MultiAgentCell : public API::TwoDCell
{

    typedef MultiAgentGrid::AgentInGrid AgentInGrid;
public:
    class AgentInCell;
    class CylinderInCell
    {
    public:
        typedef std::tr1::shared_ptr<CylinderInCell> s_ptr;
        CylinderInCell (Robot * cyl,bool access_computed=false,bool accessible=false):
            _cylinder(cyl),_accessible(accessible),_access_computed(access_computed){}
        bool isAccessible(){return _accessible;}
        bool isAccessibilityComputed(){return _access_computed;}
        void setAccessible(bool accessible){_accessible=accessible; _access_computed=true;}
        void unsetAccessibilityComputed(){_accessible=_access_computed=false;}
        Robot* getCylinder(){return _cylinder;}

    private:
        Robot* _cylinder;
        bool _accessible;
        bool _access_computed;
    };

    class MetaAgentInCell
    {
    public:
        typedef std::tr1::shared_ptr<MetaAgentInCell> s_ptr;
        MetaAgentInCell(MultiAgentGrid::MetaAgentInGrid::s_ptr meta_in_grid,CylinderInCell::s_ptr cyl,bool reach_computed=false,bool reach=false,bool boundary=false):
            _cylInCell(cyl),_reachable(reach),_reachComputed(reach_computed),_boundary(boundary), _metaAgentInGrid(meta_in_grid),letter_id(_letter_cnt++){}
        MetaAgentInCell::s_ptr shared_clone(MultiAgentGrid::MetaAgentInGrid::s_ptr meta_in_grid);
        void addRealAgent(AgentInCell* a, s_ptr s_this);
        void removeAgent(AgentInCell* a);
        bool isAccessible(){return _cylInCell->isAccessible();}
        bool isAccessibilityComputed(){return _cylInCell->isAccessibilityComputed();}
        void setAccessible(bool accessible){this->getCylinderInCell()->setAccessible(accessible);}
        void unsetAccessibilityComputed(){getCylinderInCell()->unsetAccessibilityComputed();}
        bool isReachable(){return _reachable;}
        bool isReachabilityComputed(){return _reachComputed;}
        void setReachability(bool b){_reachable=b;_reachComputed=true;}
        void unsetReachabilityComputed(){_reachable=_reachComputed=false;}
        bool isBoundary(){return _boundary;}
        void setBoundary(bool b){_boundary=b;}

        CylinderInCell::s_ptr getCylinderInCell(void){return _cylInCell;}
        MultiAgentGrid::MetaAgentInGrid::s_ptr getMetaAgentInGrid(){return _metaAgentInGrid;}

        std::vector<MetaFrontierPtr> & getFrontiers(MetaAgentInCell::s_ptr other){return _meta_frontiers[other->getMetaAgentInGrid().get()];}
        std::vector<MetaFrontierPtr> & getFrontiers(MultiAgentGrid::MetaAgentInGrid::s_ptr other){return _meta_frontiers[other.get()];}


    private:
        CylinderInCell::s_ptr _cylInCell;
        bool _reachable;
        bool _reachComputed;
        bool _boundary;
        MultiAgentGrid::MetaAgentInGrid::s_ptr _metaAgentInGrid;
        std::vector<AgentInCell*> _realAgents;
        static char _letter_cnt;
        std::map< MultiAgentGrid::MetaAgentInGrid*,std::vector<MetaFrontierPtr > > _meta_frontiers;
    public:
        char letter_id;
    };

    struct AgentInCell{
        void initFrontiersVector(unsigned int size){_frontiers.assign(size,std::vector<FrontierPtr>(0));}
        AgentInGrid* agentInGrid; 
        // traj //
        std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > vect_traj;
        std::vector<MultiAgentCell*> traj; 
        // accessibility //
        bool isAccessible(){return getMetaAgent()->isAccessible();} 
        bool isAccessibilityComputed(){return getMetaAgent()->isAccessibilityComputed();} 
        void setAccessible(bool b){getMetaAgent()->setAccessible(b);}
        void unsetAccessibilityComputed(){getMetaAgent()->unsetAccessibilityComputed();}
        // reachability //
        bool isReachable(){return  checkMetaAgent() && getMetaAgent()->isReachable();}
        bool isReachabilityComputed(){assert(checkMetaAgent()); return  getMetaAgent()->isReachabilityComputed();}
        void setReachability(bool b){assert(this->checkMetaAgent()); getMetaAgent()->setReachability(b);}
        void unsetReachabilityComputed(){assert(checkMetaAgent());getMetaAgent()->unsetReachabilityComputed();}
        // distance //
        bool distanceIsComputed; 
        double distance; 
        std::deque<double> distance_alternatives; 
        std::deque<MultiAgentCell*> origin_alternatives;
        std::deque<MultiAgentCell*> come_from_alternatives;
        // boundaries //
        bool isBoundary(void){return getMetaAgent()->isBoundary();}
        void setBoundary(bool b){getMetaAgent()->setBoundary(b);}
        std::vector<std::tr1::shared_ptr<FrontierCells> > & frontiers(AgentInCell* other){return _frontiers[other->index];}
        std::vector<MetaFrontierPtr> & getMetaFrontier(AgentInCell* other){return getMetaAgent()->getFrontiers(other->getMetaAgent());}
        std::vector<MetaFrontierPtr> & getMetaFrontier(MultiAgentGrid::MetaAgentInGrid::s_ptr other){return getMetaAgent()->getFrontiers(other);}
        // other //
        Robot * robot(void){return agentInGrid->getRobot();}
        CylinderInCell::s_ptr getCylinderInCell(void){return this->getMetaAgent()->getCylinderInCell();}
        MetaAgentInCell::s_ptr getMetaAgent(){return _metaAgentInCell;}
        void setMetaAgent(MetaAgentInCell::s_ptr meta){_metaAgentInCell=meta;metaLetter=_metaAgentInCell->letter_id;}
        bool checkMetaAgent(){return (this->agentInGrid->getMetaAgent()==this->getMetaAgent()->getMetaAgentInGrid());}
        char metaLetter;
        unsigned int index;
    private:
        MetaAgentInCell::s_ptr _metaAgentInCell;
        std::vector<std::vector<FrontierPtr > > _frontiers;
    };


public:
    MultiAgentCell();
    MultiAgentCell(int i, Eigen::Vector2i coord, Eigen::Vector2d corner, MultiAgentGrid* grid);

    ~MultiAgentCell() { }

    //#######################
    //getters et setters ####
    //#######################

    AgentInCell * getAgentAt(unsigned int i){return _agentsInCell[i];}
    unsigned int getAgentNumber(){return _agentsInCell.size();}
    MultiAgentGrid::MetaAgentInGrid::s_ptr getMetaFromGrid(unsigned int a){return this->getAgentAt(a)->agentInGrid->getMetaAgent();}

    void resetData(void);

    //distance computing
    void setDistance(unsigned int index,double value) {getAgentAt(index)->distance = value; }
    void setAltDistance(unsigned int index, double value, MultiAgentCell* alt_origin, MultiAgentCell *previous_cell=0);
    double getDistance(unsigned int index) {return getAgentAt(index)->distance; }
    double getAltDistance(unsigned int index,MultiAgentCell* alt_origin);
    void setDistanceIsComputed(unsigned int index) {getAgentAt(index)->distanceIsComputed = true;}
    void unsetDistanceIsComputed(unsigned int index) {getAgentAt(index)->distanceIsComputed = false;}
    bool isDistanceComputed(unsigned int index) {return getAgentAt(index)->distanceIsComputed;}

    double getDistanceAny(unsigned int agent,MultiAgentCell* other);
    std::vector<MultiAgentCell*> getTrajAny(unsigned int agent, MultiAgentCell *origin);
    std::vector<MultiAgentCell*> getAltTraj(unsigned int agent, MultiAgentCell *alt_origin, bool reverse=false);

    // distance propagation algorithm
    bool getOpen() { return _Open; }
    void setOpen() { _Open = true; }

    bool getClosed() { return _Closed; }
    void setClosed() { _Closed = true; }

    double getTempColor(){return _tmp_color;}
    void setTempColor(double c){_tmp_color=c;}

    void resetExplorationStatus() { _Open = false; _Closed = false; }

    // Trajectories

    std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > & getVectorTraj(unsigned int index) {return getAgentAt(index)->vect_traj;}
    void setVectorTraj(unsigned int index,std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > & vec_traj) {getAgentAt(index)->vect_traj = vec_traj; }

    std::vector<MultiAgentCell*> & getTraj(unsigned int index) {return getAgentAt(index)->traj;}
    //std::vector<Eigen::Vector2d> getTempVectorTraj(void);
    void setTraj(unsigned int index, std::vector<MultiAgentCell*> & traj) {this->getAgentAt(index)->traj = traj; }
    double getTempDist(void){return _temp_dist;}
    void setTempDist(double v){_temp_dist=v;}
    double getTempEstim(void){return _temp_estim;}
    void setTempEstim(double v){_temp_estim=v;}
    MultiAgentCell* getTempCameFrom(void) {return _temp_came_from;}
    void setTempCameFrom(MultiAgentCell * f) {_temp_came_from=f;}

    //void setAltCameFrom(unsigned int index,MultiAgentCell* origin,MultiAgentCell* previous);
    MultiAgentCell* getAltCameFrom(unsigned int index, MultiAgentCell* alt_origin);

    //accessibility
    bool isAccessible(unsigned int index){return getAgentAt(index)->getCylinderInCell()->isAccessible();}
    void setAccessible(unsigned int index,bool v){getAgentAt(index)->getCylinderInCell()->setAccessible(v);}
    bool isAccessibilityComputed(unsigned int index){return getAgentAt(index)->getCylinderInCell()->isAccessibilityComputed();}
    //void setAccessibilityComputed(unsigned int index,bool v){}//getAgentAt(index)->getCylinderInCell()->setAccessibilityComputed(v);}
    void setReachable(unsigned int index,bool b){getAgentAt(index)->setReachability(b);}
    bool isReachable(unsigned int index){return getAgentAt(index)->isReachable();}

    bool isBoundary(unsigned int index){return getAgentAt(index)->isBoundary();}
    bool isFrontier(unsigned int a1,unsigned int a2){return (getAgentAt(a1)->frontiers(getAgentAt(a2)).size() > 0);}

    bool checkIsInCollision(unsigned int agent_this,MultiAgentCell* other,unsigned int agent_other);

    std::vector<std::tr1::shared_ptr<FrontierCells> > createAllRealFrontiersFor(unsigned int a1,unsigned int a2);
    FrontierPtr createRealFrontierFor(unsigned int a1,unsigned int a2, MetaFrontierPtr mfrontier);
    std::vector<MetaFrontierPtr> & getMetaFrontier(unsigned int a1,unsigned int a2){
        assert (isReachable(a1));
        //checkMetaAgent(a2);
        return getAgentAt(a1)->getMetaFrontier(getMetaFromGrid(a2));
    }
    std::vector<MetaFrontierPtr> & getMetaFrontier(unsigned int a1,MultiAgentGrid::MetaAgentInGrid::s_ptr m2){
        assert (isReachable(a1));
        return getAgentAt(a1)->getMetaFrontier(m2);
    }
    FrontierPtr getRealFrontier(unsigned int a1,unsigned int a2,MetaFrontierPtr mfrontier);

    //other
    void setBlankCost() { _costIsComputed = false;}
    //void setCost(double value) {getAgentAt(index)->cost = value; }
    //double getCost() {return mCost; }
    //double getMoveCost(unsigned int index){return getAgentAt(index)->moveCost;}

    Eigen::Vector2i getCoord() { return _Coord; }



    //#######################
    // others ###############
    //#######################

    void computeAccessibility(unsigned int index_agent);

    // /**
    // * @brief compute move cost for an agent, according to it's speed.
    // * @param index_agent
    // * @return
    // * @todo use different weights for travel distance and duration
    // */
    //double computeMoveCost(unsigned int index_agent);
    double computeCost();

    bool computeBestRobotPos();

    bool checkMetaAgent(unsigned int index);

    void resetAccessibility();
    void resetAccessibility(unsigned int index);
    void resetDistances(unsigned int index);
    void resetAllDistances(void);

    void resetTraj();
    void resetTraj(unsigned int index);

    std::vector<MultiAgentCell*> getNeighbors(unsigned int index);

    std::vector<MultiAgentCell*> findNeighborsInRange(unsigned int agent,double range);

    double computeDist(MultiAgentCell* neighCell);

    void addPoint(double Rz);

    std::vector<MultiAgentCell*> getCrown(double min, double max);


    std::tr1::shared_ptr<MetaFrontierCells> addMetaFrontier(MultiAgentCell *other, bool over_obstacle, unsigned int agent_this,unsigned int agent_other,double dist,bool check_exists=false);
    std::tr1::shared_ptr<MetaFrontierCells> addMetaFrontier(MultiAgentCell *other, bool over_obstacle, MetaAgentInCell::s_ptr agent_this, MetaAgentInCell::s_ptr agent_other, double dist, bool check_exists=false);
    std::tr1::shared_ptr<FrontierCells> addFrontierComplete(MultiAgentCell *other, bool over_obstacle, unsigned int agent_this,unsigned int agent_other,double dist,bool check_exists=false);

private:

    std::vector<AgentInCell*> _agentsInCell;
    std::vector<CylinderInCell::s_ptr> _cylinders;
    std::vector<MetaAgentInCell::s_ptr> _metaAgents;

    Eigen::Vector2i _Coord;

    bool _Open;

    bool _Closed;

    bool _costIsComputed;

    double _tmp_color;

    MultiAgentCell * _temp_came_from;
    double _temp_dist;
    double _temp_estim;

    double _alt_distance_sum;

};

class MetaFrontierCells{
public:
    MetaFrontierCells(MultiAgentGrid::MetaAgentInGrid::s_ptr agentA,MultiAgentCell* cellA,MultiAgentGrid::MetaAgentInGrid::s_ptr agentB, MultiAgentCell* cellB,double distance,bool over_obstacle) :
        _metaA(agentA),_metaB(agentB),_cellA(cellA),_cellB(cellB), _dist(distance), _checked_ok(0), _over_obstacle(over_obstacle) {}
    MultiAgentCell * cellOf(MultiAgentGrid::MetaAgentInGrid::s_ptr meta){
        if(meta==_metaA){return _cellA;}
        if(meta==_metaB){return _cellB;}
        return 0x0;
    }
    FrontierPtr getRealFrontier (MultiAgentGrid::AgentInGrid* a1, MultiAgentGrid::AgentInGrid* a2) {
        //if (a1>a2){std::swap(a1,a2);}
        return _implementations_of_this[std::make_pair(a1,a2)];
    }
    void setRealFrontier (MultiAgentGrid::AgentInGrid* a1, MultiAgentGrid::AgentInGrid* a2,FrontierPtr frontier) {
        //if (a1>a2){std::swap(a1,a2);}
        _implementations_of_this[std::make_pair(a1,a2)] = frontier;
    }
    MultiAgentCell* getCellA(){return _cellA;}
    MultiAgentCell* getCellB(){return _cellB;}
    double getDistance(){return _dist;}
    struct CheckCellsAre{
        CheckCellsAre(MultiAgentCell* cell1,MultiAgentGrid::MetaAgentInGrid::s_ptr a1,MultiAgentCell* cell2,MultiAgentGrid::MetaAgentInGrid::s_ptr a2) :
            cell1(cell1),cell2(cell2),a1(a1),a2(a2){}
        CheckCellsAre(MetaFrontierPtr & val){cell1=val->_cellA;cell2=val->_cellB;a1=val->_metaA;a2=val->_metaB;}
        bool operator()(MetaFrontierPtr other){
            if(other->cellOf(a1) == cell1 && other->cellOf(a2) == cell2){
                return true;
            }
            return false;
        }
        MultiAgentCell* cell1,*cell2;
        MultiAgentGrid::MetaAgentInGrid::s_ptr a1,a2;
    };

    bool getCheckedOk(){return _checked_ok;}
    void setCheckedOk(bool b=true){_checked_ok=b;}
    confPtr_t getObjectPosition(){return object_position;}
    void setObjectPosition(confPtr_t q_obj){this->object_position = q_obj;}
    bool isOverObstacle(){return _over_obstacle;}

private:
    std::map<std::pair<MultiAgentGrid::AgentInGrid*,MultiAgentGrid::AgentInGrid*>, FrontierPtr> _implementations_of_this;
    MultiAgentGrid::MetaAgentInGrid::s_ptr _metaA;
    MultiAgentGrid::MetaAgentInGrid::s_ptr _metaB;
    MultiAgentCell * _cellA;
    MultiAgentCell * _cellB;
    double _dist;
    bool _checked_ok;
    confPtr_t object_position; 
    bool _over_obstacle;
};

struct FrontierCells{
    enum FrontierCheckStatus {FCS_NONE=0, FCS_OBJECT, FCS_GRAB_CONFS, FCS_HANDOVER,FCS_TRAJECTORIES,FCS_ENUM_SIZE};
    FrontierCells() : agentA(0),agentB(0),agentA_ind(0),agentB_ind(0),
        confA(),confB(),check_status(FCS_NONE),status_ok(FCS_ENUM_SIZE,false),//object_ok(0),grab_confs_ok(0),handover_ok(0),
        object_position(),grabA(),grabB(),trajA(0),trajB(0),confChecked(0),_symetric(true),
        cell(0), meta_frontier() {status_ok[FCS_NONE]=true;}
    virtual ~FrontierCells();
    MultiAgentCell * cellA(){return meta_frontier->getCellA();}
    MultiAgentCell * cellB(){return meta_frontier->getCellB();}
    MultiAgentCell::AgentInCell * agentA;
    MultiAgentCell::AgentInCell * agentB;
    unsigned int agentA_ind;
    unsigned int agentB_ind;
    confPtr_t confA;
    confPtr_t confB;
    confPtr_t r_confA,r_confB;
    double dist(){return meta_frontier->getDistance();}
    FrontierCells::FrontierCheckStatus check_status,r_check_status; 
    //bool object_ok;
    //bool grab_confs_ok;
    //bool handover_ok;
    std::vector<bool> status_ok;
    std::vector<bool> r_status_ok;
    std::tr1::shared_ptr<Configuration> object_position; 
    std::tr1::shared_ptr<Configuration> r_object_position; 
    Eigen::Vector3d grabA;
    Eigen::Vector3d grabB;
    Eigen::Vector3d r_grabA,r_grabB;
    API::Trajectory *trajA,*trajB; 
    API::Trajectory *r_trajA,*r_trajB; 
    bool confChecked;
private:
    bool _symetric; 
public:
    bool select_reverse; 
    bool isSymetric(){return _symetric;}

    void deSymetrize();

    MultiAgentCell* cellOf(unsigned int agent){
        if(agent==agentA_ind){return cellA();}
        if(agent==agentB_ind){return cellB();}
        return 0x0;
    }
    confPtr_t confOf(Robot *r){
        if(agentA->robot()==r){return confA;}
        else if(agentB->robot()==r){return confB;}
        else {confPtr_t p; return p;}
    }
    FrontierPtr neighbourFrontier(double keep_distance, double min_distance=0);
    std::vector<FrontierPtr> findNeighbourFrontiers(double keep_distance, double min_distance=0);
    std::vector<FrontierPtr> findNeighbourFrontiersJumping(double keep_distance,double min_distance,unsigned int number);
    MultiAgentCell *cell;
    MetaFrontierPtr meta_frontier;
    bool isOverObstacle(){return meta_frontier->isOverObstacle();}
};
}



#endif // HRICS_TWODGRID_HPP