#include "mag_variables.h"
#include "mag_modectrl.h"
#include "mag_controler.h"
#include "act_library.h"

#include <iostream>

#define _DEBUG

void runControler() {
    while (needtorestart.getValue()==0) {
        if ( !Robot.is_connected() ) {
            needtorestart.setValue(1);
            break;
        }
        if ( Robot.isStalled() ) {
            needtorestart.setValue(1);
            break;
        }

        #ifdef _DEBUG
        std::cerr <<"CONTROLER: Mode: "<< static_cast<int>(robotMode.getValue()) << std::endl;
        #endif

        switch ( robotMode.getValue() )
        {
        case m_randnom:
            #ifdef _DEBUG
            std::cerr <<"CONTROLER: Run: actErratic" << std::endl;
            #endif
            actErratic_Direct(Robot, MAP_L, MAP_G);
            break;
        case m_stop_or_manual:
            #ifdef _DEBUG
            std::cerr <<"CONTROLER: Run: actStopAndManual" << std::endl;
            #endif
            actStopAndManual(Robot, MAP_L, MAP_G);
            break;
        case m_pot_field:
            #ifdef _DEBUG
            std::cerr <<"CONTROLER: Run: actPotField" << std::endl;
            #endif
            actPotField_Direct(Robot, MAP_L, MAP_G, robotGoal);
            // set to stay stopped after the current goal is achieved or process terminated due to a local minimum
            if (robotMode.getValue()==m_pot_field) robotMode.setValue(m_stop_or_manual);
            break;
        case m_path:
            #ifdef _DEBUG
            std::cerr <<"CONTROLER: Run: actPath" << std::endl;
            #endif
            actPath_Direct(Robot, MAP_L, MAP_G, robotGoal);
            // set to stay stopped after the current goal is achieved or process terminated due to a local minimum
            if (robotMode.getValue()==m_path) robotMode.setValue(m_stop_or_manual);
            break;
        case m_save:
            #ifdef _DEBUG
            std::cerr <<"CONTROLER: Run: save maps" << std::endl;
            #endif
            // set to  keep stopped  so that if no new mode received while saving files
            robotMode.setValue(m_stop_or_manual);
            // save the maps
            Robot.lock();
            Robot.Stop();
            Robot.unlock();
            std::cout << "Paused for saving map files. . .";
            MAP_L.lock();
            MAP_L.write(MAP_L_fname.c_str());
            MAP_L.unlock();
            MAP_G.lock();
            MAP_G.write(MAP_G_fname.c_str());
            MAP_G.unlock();
            std::cout << " Done!" << std::endl;
            break;
        case m_quit:
            #ifdef _DEBUG
            std::cerr <<"CONTROLER: Run: actStop" << std::endl;
            #endif
            Robot.Stop();
            needtorestart.setValue(1);
            break;
        default:
            #ifdef _DEBUG
            std::cerr <<"CONTROLER: Invalid mode received, switching to random exploration" << std::endl;
            #endif
            robotMode.setValue(m_randnom);
        }

    }
    Robot.Stop();
    #ifdef _DEBUG
    std::cerr <<"CONTROLER: Exiting thread" << std::endl;
    #endif
}