EST.hpp

/*
 * EST.hpp
 *
 *  Created on: Sep 16, 2009
 *      Author: jmainpri
 */

#ifndef _EST_HPP_
#define _EST_HPP_

#include "planner/Diffusion/TreePlanner.hpp"
#include <iostream>


class ESTExpansion: public TreeExpansion
{
public:
        ESTExpansion(Graph * graph) : TreeExpansion(graph) {};

        virtual ~ESTExpansion() {};

        Node* getExpansionNode(std::vector<Node*>& nodes);

        std::tr1::shared_ptr<Configuration> getExpansionDirection(Node* expansionNode, Node* toComp);

        Node* expandProcessEST(Node* expansionNode, std::tr1::shared_ptr<Configuration> directionConfig,
                        unsigned& nbOfNodesAdded);

        void printAllNodes(std::vector<Node*>& nodes);
};


class EST: public TreePlanner
{
public:
        EST(Robot * robot, Graph * graph) : TreePlanner(robot, graph) {
                std::cout << "Construct the EST" << std::endl;
        }

        virtual ~EST() { delete expansion; }

        virtual unsigned init();

        unsigned expandOneStep(Node * fromComp);

private:
        bool checkStopConditions();

        bool preConditions();

        confPtr_t sampleExpansionDirection(Node * directionNode);

        bool connectNodeToComp(Node * node, Node * compNode);

        void addNodeToSet(Node * node);

private:
        ESTExpansion * expansion;

        std::vector<Node *> sortedNodes;
};

#endif