worldState.hpp

#ifndef WORLDSTATE_HPP
#define WORLDSTATE_HPP

#include "P3d-pkg.h"
#include "API/Device/robot.hpp"
#include "API/scene.hpp"
#include "lightPlanner/proto/ManipulationPlanner.hpp"

#include "../../planner/plannerFunctions.hpp"
#include "../lightPlanner/proto/lightPlanner.h"
#include "../lightPlanner/proto/lightPlannerApi.h"

#include "API/facts.hpp"

#include "graspPlanning/include/gpGrasp.h"
#include "taskConstraints.hpp"

#include "mightabilitiesGrids.hpp"

#include "solutionTools/structTools.hpp"

#include "jsoncpp/json/value.h"

class TaskSolution;
class TaskConstraints;
class MightabilitiesGrid;

struct Attachement;

class WorldState
{
    MOVE3D_STATIC_LOGGER;
public:
    WorldState();
    WorldState(Scene* sce);
    WorldState(WorldState* WS);
    WorldState(Json::Value root, Scene* sce);

    void load();
    void loadConf(confPtr_t q);
    void saveAll();
    void saveRobot(Robot* rob);
    void saveConf(confPtr_t q);
    bool replaceConf(confPtr_t q);
    void clear();

    bool isEqualTo(WorldState* WS);

    void addAttachement(Robot* rob, Robot* obj);
    void addAttachement(Robot* rob, Robot* obj, gpGrasp gr, int armId);
    void addAttachement(Attachement a);

    std::vector<Attachement> getAttachements() {return attachements_list;}
    void setAttachements(std::vector<Attachement> va) {attachements_list = va;}
    Attachement getAttachement(Robot* rob, Robot* obj);
    void clearAttachement();
    void removeAttachements(Robot* rob, Robot* obj);
    bool hasAttachement(Robot* rob);
    bool hasAttachement(Robot* rob,Robot* obj);

    void saveCurrentAttachement();
    void resetCurrentAttachements();
    void loadAttachements();
    Attachement getCurrentAttachment();

    std::vector<SurfaceAttachement> getSurfaceAttachements() {return surfaceAttachements_list;}
    void setSurfaceAttachements(std::vector<SurfaceAttachement> va) {surfaceAttachements_list = va;}
    void loadSurfaceAttachements();
    void clearSurfaceAttachements();

    std::vector<confPtr_t > getRobotConfs() {return _robotsConfs;}
    confPtr_t getRobotConf(Robot* rob);

    int getId() {return _id;}
//    void setId(int id) {_id = id;}

    Scene* getScene() {return _sce;}

    std::vector<Fact> getFacts();
    std::vector<Fact> getAllFacts();
    //does not compare the values of the facts
    Fact getThisFact(Fact FI);

    TaskSolution* getTSThatEndWithThisWS() {return endingStateOf;}
    std::pair<int,int> getTSIndexThatEndWithThisWS() {return endingStateOfIndex;}
    bool setTSThatEndWithThisWS(TaskSolution* TS, bool forceSetting = true);

    TaskConstraints* getTCForNextTask() {return _TCForNextTask;}
    void setTCForNextTask(TaskConstraints* tc) {_TCForNextTask = tc;}
    void clearTCForNextTask();

    void saveToDisk(FILE *file, std::string text);

    void initMightabilities();
    MightabilitiesGrid* getMightabilities();

    double distanceTo(WorldState* ws);

    void updateRobotVirtualObj(Robot* r);

    gpGrasp getGraspFromId(int graspId, Robot* object, int armId, Robot *rob);

    Json::Value toJson();
    void fromJson(Json::Value root);




private:
    std::vector<confPtr_t > _robotsConfs;
    std::vector<Attachement> attachements_list;
    std::vector<SurfaceAttachement> surfaceAttachements_list;
    //adding trajectories

    Scene* _sce;

    int _id;

    WorldStateFacts* WSF;
    bool areFactsComputed;

    TaskSolution* endingStateOf;
    std::pair<int,int> endingStateOfIndex;

    TaskConstraints* _TCForNextTask;

    MightabilitiesGrid* _MG;

    static int _idCount;

};

#endif // WORLDSTATE_HPP