LazyWAstar.h

#ifndef LAZYWASTAR_H
#define LAZYWASTAR_H

#include "Edge.h"
#include "GraphInterface.h"
#include <deque>
#include <queue>
#include "Logging/Logger.h"

namespace mho{

/*
/ **
 * @brief LWAEdgeInterface implements an interface for a Lazy Weighted A* search algorithm Edge
 *
class LWAEdgeInterface : public EdgeInterface
{
public:
    virtual bool isTrueCost() =0;
    virtual void setTrueCost(double cost) =0;
    virtual void setTmpCost(double cost) =0;
};
*/

class LWANodeInterface
{
public:
    virtual ~LWANodeInterface(){}
    virtual LWANodeInterface *clone() =0;
    //virtual std::vector<LWANodeInterface*> getClones() =0;
    virtual bool haveLowerTrueCostClones() =0;
    virtual double edgeCostEstimationFrom(LWANodeInterface *other) =0;
    virtual double computeHeuristic(LWANodeInterface* goal) =0;
    virtual double computeTrueCost(LWANodeInterface* other) = 0;
    virtual double f() =0;
    virtual double g() =0;
    virtual void g(double g) =0;
    virtual double h() =0;
    virtual void h(double h) =0;
    virtual void parent(LWANodeInterface *e) =0;
    virtual LWANodeInterface* parent() =0;

    virtual bool isTrueCost() =0;
    virtual void setTrueCost(bool t) =0;

    virtual bool isOpen() =0;
    virtual bool isClosed() =0;
    virtual void open() =0;
    virtual void close() =0;
    virtual void resetStatus() =0;
    virtual bool isSameConf(LWANodeInterface *other) =0;

    virtual std::vector<LWANodeInterface*> sons() =0;
};

class DisplaySearchAstarInterface{
public:
    DisplaySearchAstarInterface(){}
    virtual ~DisplaySearchAstarInterface() {}
    virtual void onOpen(LWANodeInterface* open){}
    virtual void onClose(LWANodeInterface* closed){}
    virtual void onFound(LWANodeInterface* goal){}
    virtual void onFail(){}
    virtual void onAbort(){}
    virtual void onEnd(std::vector<std::pair<unsigned int,double> > times){}

};

class LWANodeBase : public LWANodeInterface
{
public:
    LWANodeBase();
    LWANodeBase(LWANodeBase *other);
    virtual ~LWANodeBase();
    virtual LWANodeInterface *clone();
    void addClone(LWANodeBase* clone);
private:
    void updateOldest(LWANodeBase *new_oldest);
    void removeClone(LWANodeBase* removed);
public:
    bool haveLowerTrueCostClones();

    virtual std::vector<LWANodeInterface*> sons(){return std::vector<LWANodeInterface*>(0);}

    virtual double edgeCostEstimationFrom(LWANodeInterface *other){return -1;}
    virtual double computeHeuristic(LWANodeInterface *goal){return -1;}
    virtual double computeTrueCost(LWANodeInterface *other) {return -1;}

    virtual double f(){return g()+h();}
    virtual double g(){return _g;}
    virtual void g(double g){_g=g;}
    virtual double h(){return _h;}
    virtual void h(double h){_h=h;}
    virtual void parent(LWANodeInterface *e){_parent=e;}
    virtual LWANodeInterface* parent(){return _parent;}

    bool isTrueCost(){return _is_true_cost;}
    void setTrueCost(bool t){_is_true_cost=t;}

    bool isOpen(){return _open;}
    bool isClosed(){return _closed;}
    void open(){_open=true;}
    void close(){_closed=true;}
    void resetStatus(){_open=_closed=false;}

    virtual bool isSameConf(LWANodeInterface *other){return false;}

private:
    bool _is_true_cost;
    double _g,_h;
    bool _open,_closed;
    LWANodeInterface *_parent;
    LWANodeBase *_oldest_clone;
    std::vector<LWANodeInterface*> _clones;
};

/*
class LWAEdge :public LWAEdgeInterface, public Edge
{
public:
    LWAEdge();
    LWAEdge(NodeInterface *n1, NodeInterface *n2, double c, bool true_cost);
    virtual ~LWAEdge();

    bool isTrueCost();
    void setTrueCost(double cost);
    void setTmpCost(double cost);

private:
    bool _is_true_cost;
};
*/

class LazyWeightedAstar
{
    MOVE3D_STATIC_LOGGER;
public:
    LazyWeightedAstar(LWANodeInterface *start,LWANodeInterface *goal,double epsilon,DisplaySearchAstarInterface *interface=0,double time_limit_ms=-1,bool no_lazy=false):
        _epsilon(epsilon),_start(start),_goal(goal),_interface(0),//we set the interface later in the body
        _time_limit(time_limit_ms),_no_lazy(no_lazy)
    {
        setInterface(interface);
    }
    LazyWeightedAstar(double epsilon,DisplaySearchAstarInterface *interface=0):
        _epsilon(epsilon), _start(0),_goal(0), _interface(0), _time_limit(-1),_no_lazy(0)
    {
        setInterface(interface);
    }
    virtual ~LazyWeightedAstar();
    void reset();

    bool compute();
    bool compute(LWANodeInterface* start, LWANodeInterface *goal, double time_limit_ms=-1, bool no_lazy=false);
    std::deque<LWANodeInterface *> getPath();
    void setInterface(DisplaySearchAstarInterface *interface);
    void resetInterface(){setInterface(0);}

    double getTimeLimit(){return _time_limit;}
    void setTimeLimit(double ms){_time_limit=ms;}

private:
    struct f_value_comp
    {
        bool operator() (LWANodeInterface* n1,LWANodeInterface* n2){
            return n1->f() > n2->f();
        }
    };
    double _epsilon;
    LWANodeInterface *_start,*_goal,
    *_goal_found;   
    DisplaySearchAstarInterface *_interface;
    bool _owns_interface;
    std::priority_queue<LWANodeInterface*,std::vector<LWANodeInterface*>, f_value_comp > open;
    std::vector<LWANodeInterface*> closed;
    //GraphInterface *_graph;
    double _time_limit;
    bool _no_lazy;

};

}

#endif // LAZYWASTAR_H