HRICS_EnvGrid.hpp

#ifndef HRICS_EnvGRID_HPP
#define HRICS_EnvGRID_HPP

#include "API/planningAPI.hpp"
#include "API/Grids/gridsAPI.hpp"

namespace HRICS
{       
        class EnvCell;
        class EnvGrid : public API::TwoDGrid
        {
        public:

                EnvGrid();
                EnvGrid(double pace, std::vector<double> envSize, bool isHumanCentered);
                EnvGrid(double pace, std::vector<double> envSize, bool isHumanCentered, Robot* robot, Robot* human);

                //getters and setters
                void setRobot(Robot* R) { mRobot = R; }
                Robot* getRobot() { return mRobot; }

                void setHuman(Robot* R) { mHuman = R; }
                Robot* getHuman() { return mHuman; }

                Robot* getHumanCylinder() {return humCyl;}
                Robot* getRobotCylinder() {return robotCyl;}

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

                double getHumanMaxDist() {return m_humanMaxDist;}
                double getRobotMaxDist() { return m_robotMaxDist;}

                void setAsNotSorted() {gridIsSorted = false;}

                std::vector<EnvCell*> getHumanAccessibleCells() {return m_HumanAccessible;}

                void dumpVar();

                void init(std::pair<double,double> minMax);

                void initGrid(Eigen::Vector3d humanPos);

                void recomputeGridWhenHumanMove(Eigen::Vector3d humanPos);

                void computeHumanRobotReacheability(std::pair<double,double> minMax);

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

                void computeDistances(EnvCell* cell, bool isHuman);

                void  computeRobotDistances(EnvCell* cell);

                void  computeHumanDistances(EnvCell* cell);
                
                void draw();

                void setCellsToblankCost();

                void initAllCellState();

                void initAllTrajs();

                void initAllReachability();

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

        private:
                Robot* mRobot;

                Robot* mHuman;

                Robot* robotCyl;

                Robot* humCyl;

                double m_humanMaxDist;

                double m_robotMaxDist;

                std::vector<EnvCell*> m_HumanAccessible;

                std::vector<EnvCell*> m_RobotAccessible;

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

                bool gridIsSorted;
        };
        
        class EnvCell : public API::TwoDCell
        {
                
        public:
                EnvCell();
                EnvCell(int i, Eigen::Vector2i coord, Eigen::Vector2d corner, EnvGrid* grid);
                
                ~EnvCell() { }
                
                //#######################
                //getters et setters ####
                //#######################

                //human distance computing
                void setHumanDist(double value) {m_humanDist = value; }
                double getHumanDist() {return m_humanDist; }
                void setHumanDistIsComputed() {m_humanDistIsComputed = true;}
                bool isHumanDistComputed() {return m_humanDistIsComputed;}

                // robot distance computing
                void setRobotDist(double value) {m_robotDist = value; }
                double getRobotDist() {return m_robotDist; }
                void setRobotDistIsComputed() {m_robotDistIsComputed = true;}
                bool isRobotDistComputed() {return m_robotDistIsComputed;}

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

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

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

                // Trajectories

                std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > getHumanVectorTraj() {return humanVectorTraj;}
                void setHumanVectorTraj(std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > _humanVectorTraj) {humanVectorTraj = _humanVectorTraj; }

                std::vector<EnvCell*> getHumanTraj() {return humanTraj;}
                void setHumanTraj(std::vector<EnvCell*> _humanTraj) {humanTraj = _humanTraj; }

                std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > getRobotVectorTraj() {return robotVectorTraj;}
                void setRobotVectorTraj(std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > _RobotVectorTraj) {robotVectorTraj = _RobotVectorTraj; }

                std::vector<EnvCell*> getRobotTraj() {return robotTraj;}
                void setRobotTraj(std::vector<EnvCell*> _robotTraj) {robotTraj = _robotTraj; }

                //accessibility
                bool isHumAccessible() {return m_isHumanAccessible;}
                void setHumAccessible(bool value) {m_isHumanAccessible = value;}

                bool isRobAccessible() {return m_isRobotAccessible;}
                void setRobAccessible(bool value) {m_isRobotAccessible = value;}

                // human robot reachability (the crown)
                void setHumanRobotReachable(std::vector<EnvCell*> value) {initHumanRobotReachable = value;}
                std::vector<EnvCell*> getHumanRobotReachable() {return initHumanRobotReachable;}
                void addToHumanRobotReachable(EnvCell* cell) {initHumanRobotReachable.push_back(cell);}
                void clearHumanRobotReachable() {initHumanRobotReachable.clear();}

                void setCurrentHumanRobotReachable(std::vector<EnvCell*> value) {currentHumanRobotReachable = value;}
                std::vector<EnvCell*> getCurrentHumanRobotReachable() {return currentHumanRobotReachable;}
                void addToCurrentHumanRobotReachable(EnvCell* cell) {currentHumanRobotReachable.push_back(cell);}
                void clearCurrentHumanRobotReachable() {currentHumanRobotReachable.clear();}

                //special grid usage (slices)
                std::pair<double,EnvCell*> getRobotBestPos(){return robotBestPos;}
                void setRobotBestPos(std::pair<double,EnvCell*> value){robotBestPos = value;}

                void setAngleForHumanComming(double value) {angleForHumanComming = value;}
                double getAngleForHumanComming(){return angleForHumanComming;}

                //other
                void setBlankCost() { mCostIsComputed = false; }
                void setCost(double value) {mCost = value; }
                double getCost() {return mCost; }

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

                std::vector<double> getRandomVector(){return randomVectorPoint;}



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

                void computeHumanReach();

                void computeRobotReach();

                double computeCost();

                bool computeBestRobotPos();

                void resetReacheability();

                void resetTraj();

                std::vector<EnvCell*> getNeighbors(bool isHuman);

                double computeDist(EnvCell* neighCell);

                void addPoint(double Rz);

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

        private:
                Eigen::Vector2i _Coord;
                
                bool _Open;

                bool _Closed;
                
                bool mCostIsComputed;

                double mCost;

                bool m_isHumanAccessible;

                bool m_isRobotAccessible;

                bool m_reachComputed;

                bool m_humanDistIsComputed;

                double m_humanDist;

                std::vector<EnvCell*> humanTraj;

                std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > humanVectorTraj;

                bool m_robotDistIsComputed;

                double m_robotDist;

                std::vector<EnvCell*> robotTraj;

                std::vector<Eigen::Vector2d,Eigen::aligned_allocator<Eigen::Vector2d> > robotVectorTraj;

                std::vector<double> randomVectorPoint;

        std::vector<EnvCell*> initHumanRobotReachable;

        std::vector<EnvCell*> currentHumanRobotReachable;

                std::pair<double,EnvCell*> robotBestPos;

                double angleForHumanComming;

        };
}



#endif // HRICS_TWODGRID_HPP