configurator.h

#ifndef CONFIGURATOR_H
#define CONFIGURATOR_H
#include <dirent.h>
#include <thread>
#include <filesystem>
#include <ncurses.h>
#include <fstream>
#include <algorithm>
#include <random>
#include <sys/stat.h>
#include "debug.h"
#include "planner.h"
#include "control_interface.h"
#include "task_controller.h"
#include "tracker.h"
 
class Configurator{
protected:
    int iteration=0; //represents that hasn't started yet, robot isn't moving and there are no map data
    Task currentTask; //need to make thread safe?
    Controller * task_controller=NULL;
    Tracker * tracker=NULL;
    LIDAR_In * ci=NULL;
    Motor_Out * control=NULL;
    Planner *planner=NULL;
    Logger * logger=NULL;
    bool running =0;
    std::thread * LIDAR_thread=NULL;
    float simulationStep=2*std::max(ROBOT_HALFLENGTH, ROBOT_HALFWIDTH);
    std::chrono::high_resolution_clock::time_point previousTimeScan;
    GoalChanger * goal_changer=NULL;    
    std::vector<vertexDescriptor>m_plan, current_vertices;
    int bodies=0;
    Task controlGoal;
    CoordinateContainer data2fp;
    TransitionSystem transitionSystem=TransitionSystem(1);
    WorldBuilder worldBuilder;
    vertexDescriptor currentVertex=MOVING_VERTEX;
    edgeDescriptor movingEdge=edgeDescriptor(), currentEdge=edgeDescriptor();
 
    public:
 
 
Configurator(){
    init();
}
 
Configurator(Task _task){
    init(_task);
}
 
void init(Task _task=Task());
 
bool Spawner(); 
 
int getIteration(){
    return iteration;
}
 
void addIteration(int i=1){
    iteration+=i;
}
 
 
void dummy_vertex(vertexDescriptor src);
 
simResult simulate(Task, b2World &);
 
void estimate_current_vertex();
 
 
void printPlan(std::vector <vertexDescriptor>* p=NULL);
 
 
 
std::pair<edgeDescriptor, bool> addVertex(const vertexDescriptor & src, vertexDescriptor &v1, Edge edge=Edge(), bool topDown=0);
 
//search the TS for a plan
//std::vector <vertexDescriptor> planner(TransitionSystem&, vertexDescriptor, vertexDescriptor goal=TransitionSystem::null_vertex(), bool been=0, const Task* custom_ctrl_goal=NULL, bool * finished =NULL) ;
 
virtual void explore_plan(b2World&)=0;
 
//std::vector <vertexDescriptor> back_planner(TransitionSystem&, vertexDescriptor, vertexDescriptor root=0);
 
//starts thread
void start(); 
 
//stops thread
void stop(); 
 
void registerInterface(LIDAR_In *, Motor_Out *);
 
static void run(Configurator *);
 
// //uses LIDAR data to calculate an affine transform of disturbance Di if present
// void track_task_execution();
 
void change_task();
 
//updates environment representation with time
void update_graph(TransitionSystem& g, const b2Transform & _deltaPose);
 
//round angle to a divisor of PI/2
float approximate_angle(const float &, const Direction &, const simResult::resultType &);
 
 
void adjust_goal_expectation();
 
 
void register_controller(Controller * controller){
    task_controller=controller;
}
 
Controller* get_controller(){
    return task_controller;
}
 
void register_tracker(Tracker * _tracker){
    if (!_tracker){return;}
    tracker=_tracker;
    tracker->init(&controlGoal);
}
 
Tracker * get_tracker()const {
    return tracker;
}
 
Motor_Out * get_motor_interface(){
    return control;
}
 
LIDAR_In * get_lidar_interface(){
    return ci;
}
 
 
void register_planner(Planner * _p){
    planner=_p;
}
 
void setSimulationStep(float f){
    simulationStep=f;
}
 
void register_logger(Logger * l){
    logger=l;
}
 
static void MulT(const b2Transform& B, Task & task);
 
static void Mul(const b2Transform& B, Task &task);
 
 
protected:
 
 
};
 
class AttentiveConfigurator:public Configurator{
    protected:
    StateMatcher matcher;
ExecutionInfo package_info(vertexDescriptor gv=TransitionSystem::null_vertex(), bool been=false){
    return ExecutionInfo(currentVertex, gv, currentTask, controlGoal, been, m_plan);
}
 
Disturbance getDisturbance(TransitionSystem&g, vertexDescriptor v, b2World & world, const Direction & dir, const b2Transform& start);
 
void backtrack(std::vector <vertexDescriptor>& evaluation_q, std::vector <vertexDescriptor>&priority_q, std::set<vertexDescriptor>& closed, std::vector <vertexDescriptor>& plan_prov, vertexDescriptor module_src=MOVING_VERTEX, vertexDescriptor startRecycle=MOVING_VERTEX);
 
std::vector <vertexDescriptor> splitTask(vertexDescriptor v, Direction d, vertexDescriptor src=TransitionSystem::null_vertex());
 
void propagateD(vertexDescriptor v1, vertexDescriptor v0, std::set<vertexDescriptor>*closed=NULL, StateMatcher::MATCH_TYPE match=StateMatcher::_FALSE);
 
//if in plan the vertex gets priority
void planPriority(TransitionSystem&, vertexDescriptor); 
 
void adjust_simulated_task(const vertexDescriptor&, TransitionSystem &, Task*);
 
//adjust real-world task
void adjust_rw_task(const vertexDescriptor&, TransitionSystem &, Task*, const b2Transform &);
 
std::pair <edgeDescriptor, bool> maxProbability(std::vector<edgeDescriptor> ev, TransitionSystem&);
 
VertexMatch findMatch(State s, Direction dir=Direction::UNDEFINED, StateMatcher::MATCH_TYPE match_type=StateMatcher::_TRUE, StateDifference * _sd=NULL, std::vector <VertexMatch>*other_matches=NULL); //matches to most likely
 
std::vector<vertexDescriptor> explorer(vertexDescriptor v, TransitionSystem&g, b2World &w); //evaluates only after DEFAULT, internal one step lookahead
 
std::pair <bool, Direction> getOppositeDirection(Direction);
 
void resetPhi();
 
std::pair <edgeDescriptor, bool> add_vertex_now(const vertexDescriptor & src, vertexDescriptor & v1, Disturbance obs,Edge edge=Edge(), bool topDown=0);
 
std::pair <edgeDescriptor, bool> add_vertex_retro(vertexDescriptor &src, vertexDescriptor &v1, Edge edge=Edge(), bool topDown=0);
 
std::vector <Direction> getExploredDirections(vertexDescriptor v, const std::vector<Direction>& directions);
 
void removeExploredTransitions(vertexDescriptor v);
 
void transitionMatrix(vertexDescriptor v, Direction d, vertexDescriptor src); 
 
void applyTransitionMatrix(vertexDescriptor v0, Direction d, bool ended, vertexDescriptor src, std::vector<vertexDescriptor>& plan_prov);
 
void addToPriorityQueue(vertexDescriptor v, std::vector<vertexDescriptor>& queue, const std::set <vertexDescriptor>& closed);
 
 
//removes singleton vertices and self-edges
void ts_cleanup(TransitionSystem &, std::vector <vertexDescriptor>&);
 
//apply affine transformation to states (e.g. if the same situation encountered in the past is reencountered)
void shift_states(TransitionSystem &, const std::vector<vertexDescriptor>&, const b2Transform &); //shifts a sequence of states by a certain transform
 
//return vertex from which exploration of the environment starts (explorer)
vertexDescriptor get_explore_start(TransitionSystem &);
 
void pre_explore();
 
 
//std::vector <State> output_plan(const std::vector<vertexDescriptor> &, const TransitionSystem &);
 
void explore_plan(b2World&)override;
 
std::vector<Direction>::iterator  get_next_option(vertexDescriptor v, vertexDescriptor src, std::vector<vertexDescriptor> full_plan);
 
bool recycle_plan(vertexDescriptor v, vertexDescriptor &v0, vertexDescriptor & task_start, StateMatcher::MATCH_TYPE &matchType, 
                b2Transform & shift_start, b2Transform& sk_first_start, std::pair<edgeDescriptor, bool>&edge,
                std::vector<vertexDescriptor> &plan_prov, Direction t_get_direction);
 
 
std::pair<State, Edge> simulation_setup(b2World& w, Task & t, vertexDescriptor v0, b2Transform shift, b2Transform &start, std::vector<Direction>v0_options);
 
void reassign_direction(vertexDescriptor bestNext, Direction& direction);
 
bool matchToSafe(VertexMatch &match,const std::vector<VertexMatch> &other_matches=std::vector<VertexMatch>());
 
std::pair<edgeDescriptor, bool> setup_match_edge(VertexMatch &match, vertexDescriptor &v0, vertexDescriptor & v1,const Edge& k, Direction direction, bool changedMatch);
 
std::vector <vertexDescriptor> task_vertices(vertexDescriptor v, std::pair<bool, edgeDescriptor>* ep=NULL);
 
std::vector <vertexDescriptor> visitedOrVisitingEdge(const std::vector <edgeDescriptor>& es, TransitionSystem& g, vertexDescriptor cv=TransitionSystem::null_vertex());
 
vertexDescriptor getRecyclingStart(vertexDescriptor v, vertexDescriptor v1, vertexDescriptor taskStart);
 
std::vector <edgeDescriptor> inEdges(vertexDescriptor v, Direction d = UNDEFINED); //returns a vector containing all the in-edges of a vertex which have the specified direction
 
bool closeVertex(std::set<vertexDescriptor> & closed, vertexDescriptor v);
 
std::pair<edgeDescriptor, bool> addEdgeRetrospectively(vertexDescriptor v, vertexDescriptor &v1, const State & s_tmp,std::pair<edgeDescriptor, bool> first_edge, Direction d, float linearSpeed);
 
void correctQueue(std::vector<vertexDescriptor>& queue, vertexDescriptor v, vertexDescriptor startRecycle, int planProvSize);
 
void adjustProbability(const edgeDescriptor &e);
 
public:
 
AttentiveConfigurator(){};
 
AttentiveConfigurator(Task _task){
    init(_task);
}
 
 
};
 
 
class ReactiveConfigurator:public Configurator{
    protected:
    void explore_plan(b2World&)override;
    public:
 
    ReactiveConfigurator(){};
 
    ReactiveConfigurator(Task _task){
        init(_task);
}
    
 
};
 
 #endif