Sem_PlanParts.hpp

#ifndef SEM_PLANPARTS_H
#define SEM_PLANPARTS_H

#include <jsoncpp/json/json.h>

#include "API/Device/robot.hpp"
#include "Logging/Logger.h"
#include "Sem_AgentType.hpp"


namespace Sem {

class PlanningType
{
public:
    PlanningType();
    PlanningType(std::string s);
    virtual ~PlanningType();
    std::string toString() const;
    bool isDefined() const {return _value >=0;}

    friend bool operator==(PlanningType const &p1, PlanningType const &p2);
    friend bool operator!=(PlanningType const &p1, PlanningType const &p2);
    friend bool operator<(PlanningType const &p1, PlanningType const &p2);
    friend bool operator>(PlanningType const &p1, PlanningType const &p2);

    static std::vector<PlanningType> currentlyDefinedPlanningTypes();

protected:
    static std::vector<std::string> __strs;
private:
    int _value;
};

class PlanningPart
{
    MOVE3D_STATIC_LOGGER;
public:
    PlanningPart();
    PlanningPart(AgentType agent_type,PlanningType plan_type,const std::vector<int> &joint_ids,const std::vector<int> & arm_ids);
    PlanningPart(std::vector<PlanningPart*> planning_parts);
    virtual ~PlanningPart();

    void concat(const PlanningPart &p);

    std::vector<int> getAllJointIds() const;
    std::vector<int> getJointsOfArmId(int armId) const;

    static PlanningPart* createFromActiveJoints(PlanningType plan_type);

    PlanningType planType() const;
    void setPlanType(const PlanningType &planType);

    const std::vector<int> &jointsIndices() const;
    void setJointsIndices(const std::vector<int> &jointsIndices);


    AgentType agentType() const;
    void setAgentType(const AgentType &agentType);

    const std::vector<int> &armIds() const;
    void setArmIds(const std::vector<int> &armIds);

    static Robot* getRobotOfType(AgentType t);

    bool operator==(PlanningPart const &other) const ;
    bool operator!=(PlanningPart const &other) const ;

private:
    AgentType _agentType;
    PlanningType _planType;
    std::vector<int> _jointsIndices;
    std::vector<int> _armIds;
};

class PlanParts
{
    MOVE3D_STATIC_LOGGER;
public:

    PlanParts( std::string global_json_path,bool proactive=false);
    ~PlanParts();

    bool fetchData();
    void clearData(AgentType t);
    bool importJsonFile(std::string path);

    bool createFromActiveJoints(PlanningType plan_type);

    bool getJsonRoot(Json::Value &root,std::string &file_path);
    bool readJsonAgent(AgentType t);
    bool readJsonAgent(Json::Value &root,const std::string &file);
    bool readJsonPlanType(AgentType agn_type,PlanningType plan_type);
    bool parsePlanPart(AgentType agn_type, std::string const &plan_type_str, Json::Value const &joint_set_json);
    bool parsePlanPart(AgentType agn_type, PlanningType plantype, Json::Value const &joint_set_json);

    void save();
    void writeJson(AgentType t);
    void appendJointsSetToJson(Json::Value &planning_parts_dict,  const PlanningPart *jointsset);

    std::string getPath(AgentType t);

    void addPlanPart(AgentType agent_type, PlanningType plan_type, std::vector<int> const & joints, std::vector<int> const & arm_ids);
    void addPlanPart(PlanningPart * jointsset);

    std::vector<PlanningPart *> getPlanningParts(AgentType agent_type, PlanningType plan_type);

    //std::vector<int> getPlanPart(HRI_AGENT_TYPE agent_type, PlanningType plan_type, bool *found=0,bool *have_other=0);

    // static std::string planningTypeToString(PlanningType t);
    // static PlanningType stringToPlanningType(std::string const &s);

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

private:
    //static std::map<PlanningType,std::string> _planningTypeStrings;

private:
    bool _proactive;
    std::string _global_json_path;
    std::map<AgentType, std::map<PlanningType,std::vector<PlanningPart *> > > _planParts;
};

} // namespace Sem

#endif // SEM_PLANPARTS_H