#include "mag_variables.h"
#include "mag_sensors.h"
#include <cstdlib>
#include <iostream>

void runSensors() {
    char buffer[1400];                      // for optimum network performance do not send more than 1400 bytes in one message
    while (needtorestart.getValue()==0) {
        ArUtil::sleep(500);                 // this delay must be large, see corresponding comment in database.cpp
        // do the routine tasks for this thread

        // send the buffer with recent sensor readings and empty it
        Robot.SMlock();                     // if forget to lock the exported data may be corrupt
        const NAV_Robot::pVectArPWT MV = Robot.SMgetCumulativeBufferAsVector(); // this is a pointer to vector<ArPoseWithTime>
        // Note: this is actually   const std::vector<ArPoseWithTime>*
        Robot.SMclearCumulativeReadings();  // all points are now gone from the sonar (and the list in example 1, only still kept in the detached vector
        Robot.SMunlock();                   // if forget to unlock then robot cannot pump new reading into the sensor and all interface may halt

        int recentpointstored = 0;
        int recentlinesstored = 0;
        // let us place the points read by sensors on the map as occupied
        // Note: the iterator below is actually   std::vector<ArPoseWithTime>::iterator
        for (NAV_Robot::itVectArPWT it=MV->begin(); it!=MV->end(); ++it) {
            ArPose rpose;
            if ( Robot.getPoseAt(it->getTime(), rpose) == true
                && rpose.getX()==rpose.getX() && rpose.getY()==rpose.getY()
                // ARIA library has an error that results in NaN value for some robot past positons
                // Since NaN!=NaN the check above filters out such cases from .
               ) {

                // Add obstacle, empty robot position at that time, empty the line of sight
                sprintf_s(buffer, sizeof(buffer), "E %s %.3lf %.3lf %.3lf %.3lf %.3lf", deviceID.c_str(), it->getX(), it->getY(), rpose.getX(), rpose.getY(), rpose.getTh());

                // Map.lock();
                // MAP.addLineToObstacle(*it, rpose);
                // Map.unlock();
            } else {
                // sensor obstacle (robot postion unknown at the time of echo)
                sprintf_s(buffer, sizeof(buffer), "O %s %.3lf %.3lf", deviceID.c_str(), it->getX(), it->getY());
                // Map.lock();
                // MAP.addObstacle(*it);
                // Map.unlock();

                // we could mark clear path of all sensors with MAP.addLineToFree(..);
                // as it is rather unlikely that no sensors generated readings over some time
                // with obstacles present around the robot

        // let us make sure that the place where the robot is is marked as free/erased
        // (we should cover the whole robot not just its center)
        // send current position and possibly some more informatrion about the state
        sprintf_s(buffer, sizeof(buffer), "P %s %.3lf %.3lf %.3lf", deviceID.c_str(), Robot.getX(), Robot.getY(), Robot.getTh());
        // Map.lock();
        // Robot.lock();
        // MAP.addFreeAddRobot(Robot.getPose());
        // Robot.unlock();
        // Map.unlock();


    #ifdef _DEBUG
    std::cerr <<"SENSORS: Exiting thread" << std::endl;