#include "mag_variables.h"
#include "mag_modectrl.h"
#include "mag_controler.h"
#include "mag_sensors.h"
#include "mag_database.h"

#include <process.h>                    /* needed for _beginthread(...)   */
#include <conio.h>                      /* needed for _kbhit() _getchar() */
#include <iostream>


int main()
{
    InitRobot();

    while(robotMode.getValue()!=m_quit) {
        if (!Robot.is_connected()) Robot.connect();
        if (!Robot.is_connected()) {
            std::cerr << "Lost connection, will try again in a few seconds. . ." << std::endl;
            ArUtil::sleep(5000);
            continue;
        }

        int status=NetComStart();
        if (status!=0) {
            std::cerr << "Cannot connect to network, will try again in a few seconds. . ." << std::endl;
            ArUtil::sleep(1000);
            continue;
        }

        MAP_L.lock();
        MAP_L.clear();
        MAP_L.unlock();
        std::cout << std::endl << "Reading global map. . . ";
        MAP_G.lock();
        if (MAP_G.read(MAP_G_fname.c_str(), 0x5F))  // 0x5F means that for bits -RAUUUUU only -0AUUUUU are read from file
                                                    // i.e. any robot trails are read only as other robots, not us
            std::cout << "Global  map read." << std::endl;
        else
            std::cout << "Reading of the global map failed, continuing with a new map." << std::endl;
        MAP_G.unlock();

        // Assuming that we are already located, if not, then move this to mag_database.cpp
        // to the point when we successfully locate our platform on a global map
        const size_t max_threads = 3;
        HANDLE threads[max_threads] = {NULL};
        threads[0] = (HANDLE) _beginthread( (void (*)(void *)) runSensors       , 0, NULL);
        threads[1] = (HANDLE) _beginthread( (void (*)(void *)) runDatabase      , 0, NULL);
        threads[2] = (HANDLE) _beginthread( (void (*)(void *)) runControler     , 0, NULL);

        while (needtorestart.getValue()==0) {
            ArUtil::sleep(100);     // forever loop with low CPU usage
        }

        WaitForMultipleObjects(max_threads, threads, true, INFINITE);
        for (size_t i=0; i<max_threads; i++)
            if (threads[i]!=NULL)
                CloseHandle(threads[i]);
        NetComStop();

        if (Robot.is_connected()) Robot.disconnect();
        else std::cerr << "Lost connection, will try again in a few seconds. . ." << std::endl;

        std::cout << "Saving the map files. . ." << std::endl;
        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;
    }


    return(0);
}