gtpPlan.hpp

#ifndef GTPPLAN_HPP
#define GTPPLAN_HPP


#include "GTP/GTPTools/worldState.hpp"
#include "onePlan.hpp"
#include "planNode.hpp"
#include "onePlan.hpp"
#include <iostream>
#include <fstream>

class OnePlan;

class PlanTree
{
    MOVE3D_STATIC_LOGGER;
public:
    PlanTree();
    ~PlanTree();

    bool checkIfPlanInTree(OnePlan* OP);
    OnePlan* getPlanFromNode(PlanNode* PN);
    PlanNode* getNodeFromId(int idFirst, int idSecond);
    PlanNode* getNodeFormTS(TaskSolution* TS);

    bool addPlanNode(PlanNode* PN, TaskSolution *prevTS);

    void printAll();

    void printInFile(PlanNode* pn);

    std::vector<PlanNode*> getAllNodes() {return _tree;}

    double computeDist(PlanNode* p1, PlanNode* p2);

    PlanNode* getClosestNode(WorldState* ws, type_data td );

    void saveInFile(std::string path);


private:
    std::vector<PlanNode*> _tree;
    std::string myfilepath;

public:
    std::vector<double> getAllImpoInfo();
};


#endif // GTPPLAN_HPP