#include "nav_robot2map.h"

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

    MAP.lock();
    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 robot;
        if ( R.getPoseAt(it->getTime(), robot) == true
            && robot.getX()==robot.getX() && robot.getY()==robot.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
               MAP.addLineToObstR(*it, robot);
            recentlinesstored++;
        } else {
            MAP.addObstacle(*it);
            // 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
            recentpointstored++;
        }
    }

    // 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)
    MAP.addFreePointR(R.getPose());
    MAP.unlock();

    return(recentlinesstored+recentpointstored);
}