facts.hpp

#ifndef FACTS_HPP
#define FACTS_HPP

#include "P3d-pkg.h"
#include "move3d-headless.h"
#include "Planner-pkg.h"

#include "Device/robot.hpp"
#include "scene.hpp"





class WorldStateFacts;
class Fact;
//class Scene;

typedef struct struct_fact_id{
    std::string factType;
    std::string factSubType;
    bool isMainAnAgent;
} fact_id;


class FactsManager
{
    MOVE3D_STATIC_LOGGER;
public:
    FactsManager();
    FactsManager(Scene *scen);
    ~FactsManager();

    void initAll();
    bool disableFacts(std::string s);
    bool enableFacts(std::string s);
    bool isFactEnabled(std::string s);

    bool computeVisibility(WorldStateFacts* WS,Robot* mainEntity, Robot* targetEntity, bool forceRecompute);
    bool computeReachability(WorldStateFacts* WS,Robot* mainEntity, Robot* targetEntity, bool forceRecompute);
    bool computeIsOn(WorldStateFacts* WS,Robot* mainEntity, Robot* targetEntity, bool forceRecompute);
    bool computeIsIn(WorldStateFacts* WS,Robot* mainEntity, Robot* targetEntity, bool forceRecompute);
    bool computeIsNextTo(WorldStateFacts* WS,Robot* mainEntity, Robot* targetEntity, bool forceRecompute);
    bool computeSize(WorldStateFacts* WS,Robot* mainEntity, Robot* targetEntity, bool forceRecompute);
    bool computeClutter(WorldStateFacts* WS,Robot* mainEntity, Robot* targetEntity, bool forceRecompute);


    bool computeRobotVisibility(WorldStateFacts* WS, Robot* mainEntity, bool forceRecompute);
    bool computeRobotReachability(WorldStateFacts* WS, Robot* mainEntity, bool forceRecompute);
    bool computeRobotIsOn(WorldStateFacts* WS, Robot* mainEntity, bool forceRecompute);
    bool computeRobotIsIn(WorldStateFacts* WS, Robot* mainEntity, bool forceRecompute);
    bool computeRobotIsNextTo(WorldStateFacts* WS, Robot* mainEntity, bool forceRecompute);
    bool computeRobotSize(WorldStateFacts* WS, Robot* mainEntity, bool forceRecompute);
    bool computeRobotClutter(WorldStateFacts* WS, Robot* mainEntity, bool forceRecompute);


    bool computeAllVisibilities(WorldStateFacts* WS, bool forceRecompute);
    bool computeAllReachabilities(WorldStateFacts* WS, bool forceRecompute);
    bool computeAllIsOn(WorldStateFacts* WS, bool forceRecompute);
    bool computeAllIsIn(WorldStateFacts* WS, bool forceRecompute);
    bool computeAllIsNextTo(WorldStateFacts* WS, bool forceRecompute);
    bool computeAllSize(WorldStateFacts* WS, bool forceRecompute);
    bool computeAllClutter(WorldStateFacts* WS, bool forceRecompute);

    bool computeFacts(WorldStateFacts* WS, bool forceRecompute);
    bool computeFacts(int WSid, bool forceRecompute);
    int computeFacts(bool forceRecompute);


    int findWorldStateFacts(WorldStateFacts* WS);
    int findOrAddWorldStateFacts(WorldStateFacts* WS);
    int findCurWorldStateFacts();

    void setFactToFalse(WorldStateFacts* WS, fact_id ft, Robot* mainEntity, Robot* targetEntity);

    bool cleanWSList();

    bool computeOneFact(WorldStateFacts* WSF, std::string fact, Robot *mainEntity);
    std::vector<Fact> computeAndGetFact(std::string fact, Robot *mainEntity);
    Fact computeAndGetFact(std::string fact, std::string factSubType, Robot *mainEntity, Robot* targetEntity);

    WorldStateFacts* getWSFromId(int WSid);

    std::vector<fact_id> getFactList() {return factList;}

private:
    static std::vector<fact_id> factList;
    std::vector<std::pair<int, WorldStateFacts*> > WSlist;
    int WSlistId;
    Scene* sce;

    std::vector<std::string> disabledFacts;

};



class Fact
{
    MOVE3D_STATIC_LOGGER;
public:
    Fact();
    Fact(fact_id fact_type);

    bool compareFactsDescr(Fact f);
    bool compareFactsDescr(fact_id FI, Robot* main, Robot* target);
    bool compareFactsValue(Fact f);

    fact_id getType();

    Robot* getMainEntity();
    Robot* getTargetEntity();

    void setMainEntity(Robot* r);
    void setTargetEntity(Robot* r);

    bool isItComputed();
    void setIsComputed(bool t);

    int getValueType();
    void setValueType(int v);

    bool getBoolValue();
    void setBoolValue(bool v);

    double getSpecValue();
    void setSpecValue(double v);

    void printFact();

    std::string getFactDescr();


private:
    fact_id fType;

    Robot* mainEntity;
    Robot* targetEntity;

    bool isComputed;

    int value_type; //0 for bool, 1 for specificValue

    bool boolValue;
    double specificValue;


};



class WorldStateFacts
{
    MOVE3D_STATIC_LOGGER;
public:
    WorldStateFacts();

    WorldStateFacts(Scene *scen);
    WorldStateFacts(std::vector<confPtr_t> confs );
    std::vector<Robot*> compareWS(WorldStateFacts* WS);

    void printAllFacts();
    void printAllComputedFacts();
    void printFacts(std::vector<Fact> fp);

    bool initFacts(std::vector<fact_id> factList);


    std::vector<Fact> getFacts();
    std::vector<Fact> getMeaningfulFacts();
    std::vector<Fact> getUpdatedFacts(WorldStateFacts* WS);
    void setFacts(std::vector<Fact> Facts);

    std::vector<Fact> getFactFromMain(std::string factType, std::string main);
    std::vector<Fact> getFactFromTarget(std::string factType, std::string target);

    Fact getFact(fact_id Fact_Id, Robot* main, Robot* target);
    Fact getFact(std::string factType, std::string main, std::string target);
    void setFact(Fact fact);

    std::vector<confPtr_t> getRobotsConfs();

    confPtr_t getRobotConf(Robot* robot);

    confPtr_t  getRobotConfById(int robot_id);

    void setConfigurations(std::vector<confPtr_t> robVect) { robotsConfs = robVect;}
    void setConfiguration(confPtr_t conf);

private:
    std::vector<confPtr_t> robotsConfs;
    std::vector<Fact> facts;

    Scene* sce;
};




#endif // FACTS_HPP