localpath.hpp

#ifndef LOCALPATH_HPP
#define LOCALPATH_HPP

#include "API/ConfigSpace/configuration.hpp"

#include "Localpath-pkg.h"

class Robot;

class LocalPath
{
public:
        LocalPath(std::tr1::shared_ptr<Configuration> B, std::tr1::shared_ptr<Configuration> E);

        LocalPath(LocalPath& path, double& pathDelta, bool lastValidConfig = true);

        LocalPath(const LocalPath& path);

        LocalPath(Robot* R, p3d_localpath* lpPtr);

        ~LocalPath();

        //Accessor
        p3d_localpath* getLocalpathStruct(bool multi_sol = false);

        std::tr1::shared_ptr<Configuration> getBegin();
        std::tr1::shared_ptr<Configuration> getEnd();

        //      Graph* getGraph();
        Robot* getRobot();

        int getNbColTest();

        int getNbCostTest();
    void setNbCostTest(double nbCostTest){_NbCostTest = nbCostTest;}

        bool getEvaluated();

        p3d_localpath_type getType();

        std::tr1::shared_ptr<Configuration> getLastValidConfig(double& p);

        void setLocalpathAsNotTested() {
                _Evaluated = false;
        }

        bool isValid();

        bool unvalidLocalpathTest(Robot* R, int* ntest);

        bool classicTest();

        double length();

        double getParamMax();

        std::tr1::shared_ptr<Configuration> configAtDist(double dist);

        std::tr1::shared_ptr<Configuration> configAtParam(double param);

        double stayWithInDistance(double u, bool goForward, double* distance);

        double getResolution(double step = 0.0);

        unsigned int getNumberOfCostSegments();

        std::vector<std::pair<double, double> > getCostProfile();

        double whenCostIntegralPasses(double thresh);

        double cost();

    double getAverageCost();
    void setAverageCost(double averageCost){_AverageCost=averageCost;}

        void setCost(double cost) { _Cost = cost; _costEvaluated = true; }
    void setMaxCost(double cost){_MaxCost = cost;}
    double getMaxCost() const;

        double getReverseCost() const { return reverseCost; }

        void setReverseCost(double cost) { reverseCost = cost; }

        void resetCostComputed() { _costEvaluated = false; }

        void setIkSol(int* iksol) { _ikSol = iksol; }

        std::tr1::shared_ptr<LocalPath> buildReversePath();

        void print();

protected:
        Robot* _Robot;

        std::tr1::shared_ptr<Configuration> _Begin;
        std::tr1::shared_ptr<Configuration> _End;

private:

        p3d_localpath* _LocalPath;
        bool _Valid;
        bool _Evaluated;
        double _lastValidParam;
        std::tr1::shared_ptr<Configuration> _lastValidConfig;
        bool _lastValidEvaluated;
        int _NbColTest;

        int* _ikSol;

    bool _costEvaluated;
    double _Cost;
        bool _ResolEvaluated;
        double _Resolution;

        p3d_localpath_type _Type;



    double _AverageCost;
    double _MaxCost;
        int _NbCostTest;

    double reverseCost;
};

typedef std::tr1::shared_ptr<LocalPath> localPathPtr_t;

#endif