HRICS_CostManager.h

#ifndef HRICS_COSTMANAGER_H
#define HRICS_COSTMANAGER_H


#include "Logging/Logger.h"
#include "database/semantics/Sem_ActionsManager.hpp"
#include <boost/function.hpp>
#include <boost/bind.hpp>
#include "API/ConfigSpace/localpath.hpp"
#include "HRICS_CostSettingType.h"

namespace HRICS {

class CostManager
{
    MOVE3D_STATIC_LOGGER;
public:
    CostManager();
    CostManager(const std::string &global_path, bool proactive=false);
    ~CostManager();
    static CostManager * mainCostManager();

    void takeControlAndSet();

    void setActionType(Sem::ActionType act_t);
    void setCostType(CostSettingType cost_t);
    void updatePlannerSettings();

    double getCost(Configuration &q);
    double getCostPath(std::vector<LocalPath*>& path);
    double getCostAggregationNormalized(std::vector<LocalPath*> &path);

    boost::function<double (std::vector<LocalPath*>&)> getCostPathFunction(CostSettingType type);

    void clear();

    void prepareForSmoothing(API:: Trajectory &path);
    void endSmoothing(API::Trajectory *path=0);


    typedef enum{CONF_COST,PATH_COST,PATH_SMOOTHING_COST} CostAppliesWhen_t;

    bool fetchSettings();
    bool readActionsCostMapping(std::string path);
    bool readCostSetting(const CostSettingType &cost, CostAppliesWhen_t when);
    bool readCostSetting(std::string path,const CostSettingType &cost,CostAppliesWhen_t when);

    bool readAlgoParams(const Sem::AgentType &agent_type);
    bool readAlgoParams(const Sem::AgentType &agent_type, const std::string &path);

    boost::function<double(Configuration&)> jsonToFunction(Json::Value &f);
    static boost::function<double(double,double)> elementaryOperation(std::string const &name);
    static boost::function<double(double)> elementaryFunction(std::string const &name, const Json::Value &json);
    boost::function<double(std::vector<LocalPath*>&)> jsonToFunctionTraj(Json::Value &f);

    std::string getPath();
    std::string getPath(CostAppliesWhen_t when);


    bool proactive() const;
    void setProactive(bool proactive);

    std::string globalPath() const;
    void setGlobalPath(const std::string &globalPath);

    CostSettingType currentCostType() const;

    bool isSmoothing() const;

    double pathCostNormalizationFactor() const;
    void setPathCostNormalizationFactor(double pathCostNormalizationFactor);

    static double sigmoidFunction(double val, double upper, double lower, double min, double max);
    static double boundFunction(double val,double min,double max);
    static double affineFunction(double val,double a,double b);

    Env::boolParameter planningMethod();

    bool hasControlOnPlanning() const;
    void setHasControlOnPlanning(bool hasControlOnPlanning);

    std::map<std::string, double> const &functionCostStack() const;

protected:
    double getCostStack(std::string const &name,Configuration &q);
    double getCostStackPath(std::string const &name,std::vector<LocalPath*>& path);


private:
    bool _proactive;
    bool _hasControlOnPlanning;

    std::string _globalPath;

    std::map<Sem::ActionType,CostSettingType> _actionCostMapping;
    std::map<CostSettingType,boost::function<double(Configuration&)> > _functions;

    std::map<CostSettingType,boost::function<double(std::vector<LocalPath*>&)> > _functionsTraj;
    std::map<CostSettingType,boost::function<double(std::vector<LocalPath*>&)> > _functionsTrajSmoothing;

    std::map<std::string,double> _functionCostStack;
    bool _lockFunctionsCostStack;
    std::map<std::string,double> _functionCostStackPath;
    bool _lockFunctionsCostStackPath;

    struct AlgoParams {
        AlgoParams():extStep(0),finalGap(0){}
        AlgoParams(double extStep,double finalGap):extStep(extStep),finalGap(finalGap){}
        double extStep,finalGap;
    };

    void updatePlannerSettings(AlgoParams &params);
    std::map<Sem::AgentType,AlgoParams> _algoParams;

    CostSettingType _currentCostType;

    static CostManager* __mainCostManger;

    bool _isSmoothing;

    double _pathCostNormalizationFactor;

};

} // namespace HRICS

#endif // HRICS_COSTMANAGER_H