#include "mag_variables.h"
#include "mag_modectrl.h"
#include "mag_database.h"
#include <iostream>
#include <strstream>

void runDatabase() {
    char buffer[1400];
    while (needtorestart.getValue()==0) {
        // do the routine tasks for this thread

        int status;
        status=NetComRecv(buffer,sizeof(buffer));
        if (status==0) {            // no data received
            continue;
        } else if (status<0) {      // problem with socket
            needtorestart.setValue(1); // restart on socket error including timeout
                                    // (we should be at least sending data to ourself)
            continue;
        }
        buffer[1399]='\0'; // just in case terminate the string

        #ifdef _DEBUG
        std::cout << "DATABASE: " << buffer << std::endl;
        #endif

        unsigned long now = CurrentTime();

        if (buffer[0]=='P') {
            char c;
            std::string inID;
            double inRX=0, inRY=0, inRT=0;
            std::istrstream ISTR(buffer);
            ISTR >> c >> inID >> inRX >> inRY >> inRT;
            if (ISTR.fail()) {
                #ifdef _DEBUG
                std::cout << "DATABASE: Invalid message: " << buffer << std::endl;
                #endif
                continue;
            }
            ArPose inPO(inRX, inRY, inRT);

            if (inID==deviceID) {               // it is us
                if (KnownRobots[inID].located) {
                    // ** translate inPO here **
                    // ** continuously attempt finding/updating translation in a separate thread **
                    KnownRobotsLock.lock();
                    KnownRobots[inID].position  = inPO;
                    KnownRobots[inID].timestamp = now;
                    KnownRobotsLock.unlock();
                } else {
                    // ** attempt translation calculation **
                    // at this moment we do not calculate translation, assume that it is true zero
                    KnownRobotsLock.lock();
                    KnownRobots[inID].position  = inPO;
                    KnownRobots[inID].timestamp = now;
                    KnownRobotsLock.unlock();
                }
            } else {
                if (KnownRobots.find(inID)==KnownRobots.end()) { // new agent found
                    #ifdef _DEBUG
                    std::cout << "* New agent detected ID=" << inID << std::endl;
                    #endif
                    // at this moment we do not calculate translation, assume that it is true zero
                    KnownRobotsLock.lock();
                    KnownRobots[inID] = RobotState(true, ArPose(0,0,0), inPO, now);
                    KnownRobotsLock.unlock();
                } else {                        // existing agent
                    if (KnownRobots[inID].located) {
                        // ** translate inPO here **
                        // ** continuously attempt finding/updating translation in a separate thread **
                        KnownRobotsLock.lock();
                        KnownRobots[inID].position  = inPO;
                        KnownRobots[inID].timestamp = now;
                        KnownRobotsLock.unlock();
                    } else {
                        // ** store it with hope that we will be able to translate it in the future **
                        KnownRobotsLock.lock();
                        KnownRobots[inID].position  = inPO;
                        KnownRobots[inID].timestamp = now;
                        KnownRobotsLock.unlock();
                    }
                }
            }

            if (inID==deviceID) {
                MAP_L.lock();
                MAP_L.addFreePointR(inPO);
                MAP_L.unlock();
                MAP_G.lock();
                MAP_G.addFreePointR(inPO);
                MAP_G.unlock();
            } else {
                if (KnownRobots[inID].located) {
                    // at this moment we do not update translation, assume that it is zero
                    // ** otherwise translate inPO here **
                    MAP_G.lock();
                    MAP_G.addFreePointAR(inPO);
                    MAP_G.unlock();
                } else {
                    // ** store it with hope that we will be able to translate it in the future **
                    MAP_G.lock();
                    MAP_G.addFreePointAR(inPO);
                    MAP_G.unlock();
                }
            }

            // erase outdated agent information
            for (std::map<std::string, RobotState>::iterator inIT=KnownRobots.begin(); inIT!=KnownRobots.end(); ++inIT) {
                if ( now - inIT->second.timestamp > max_robot_age && inIT->first!=deviceID ) {
                    // delete any device information that has not been tracked for more than max_robot_age seconds
                    // but do not delete my own device (it should never be old though)
                    #ifdef _DEBUG
                    std::cout << "* Agent left ID=" << inIT->first << std::endl;
                    #endif
                    // ** do you need to do anything else about updating robot that vanished? **
                    KnownRobots.erase(inIT);
                    break; // we cannot continue with the same iterator, we will purge further next time
                }
            }

        } else if (buffer[0]=='E') {
            char c;
            std::string inID;
            double inOX=0, inOY=0, inRX=0, inRY=0, inRT=0;
            std::istrstream ISTR(buffer);
            ISTR >> c >> inID >> inOX >> inOY >> inRX >> inRY >> inRT;
            if (ISTR.fail()) {
                #ifdef _DEBUG
                std::cout << "DATABASE: Invalid message: " << buffer << std::endl;
                #endif
                continue;
            }
            ArPose inRP(inRX, inRY, inRT);
            ArPose inOP(inOX, inOY, 0);
            if (inID==deviceID) {
                if (KnownRobots[inID].located) {
                    // at this moment we do not update translation, assume that it is zero
                    // ** otherwise translate inPO here **
                    // ** continuously attempt finding/updating translation in a separate thread **
                    MAP_L.lock();
                    MAP_L.addLineToObstR(inOP,inRP);
                    MAP_L.unlock();
                    MAP_G.lock();
                    MAP_G.addLineToObstR(inOP,inRP);
                    MAP_G.unlock();
                } else {
                    // ** atttempt collecting data so that it can be eventually localized **
                    // ** continuously attempt finding/updating translation in a separate thread **
                }
            } else {
                if (KnownRobots.find(inID)!=KnownRobots.end()) { // pursue only if already registered
                    if (KnownRobots[inID].located) {
                        // at this moment we do not update translation, assume that it is zero
                        // ** otherwise translate inPO here **
                        // ** continuously attempt finding/updating translation in a separate thread **
                        MAP_G.lock();
                        MAP_G.addLineToObstAR(inOP,inRP);
                        MAP_G.unlock();
                    } else {
                        // ** atttempt collecting data so that it can be eventually localized **
                        // ** continuously attempt finding/updating translation in a separate thread **
                    }
                }
            }
        } else if (buffer[0]=='O') {
            char c;
            std::string inID;
            double inOX=0, inOY=0;
            std::istrstream ISTR(buffer);
            ISTR >> c >> inID >> inOX >> inOY;
            if (ISTR.fail()) {
                #ifdef _DEBUG
                std::cout << "DATABASE: Invalid message: " << buffer << std::endl;
                #endif
                continue;
            }
            ArPose inOP(inOX, inOY, 0);
            if (inID==deviceID) {
                if (KnownRobots[inID].located) {
                    // at this moment we do not update translation, assume that it is zero
                    // ** otherwise translate inPO here **
                    // ** continuously attempt finding/updating translation in a separate thread **
                    MAP_L.lock();
                    MAP_L.addObstacle(inOP);
                    MAP_L.unlock();
                    MAP_G.lock();
                    MAP_G.addObstacle(inOP);
                    MAP_G.unlock();
                } else {
                    // ** atttempt collecting data so that it can be eventually localized **
                    // ** continuously attempt finding/updating translation in a separate thread **
                }
            } else {
                if (KnownRobots.find(inID)!=KnownRobots.end()) { // pursue only if already registered
                    if (KnownRobots[inID].located) {
                        // at this moment we do not update translation, assume that it is zero
                        // ** otherwise translate inPO here **
                        // ** continuously attempt finding/updating translation in a separate thread **
                        MAP_G.lock();
                        MAP_G.addObstacle(inOP);
                        MAP_G.unlock();
                    } else {
                        // ** atttempt collecting data so that it can be eventually localized **
                        // ** continuously attempt finding/updating translation in a separate thread **
                    }
                }
            }
        } else if (buffer[0]=='G'||buffer[0]=='W') {
            char c;
            std::string inID;
            double inRX=0, inRY=0, inRT=0;
            std::istrstream ISTR(buffer);
            ISTR >> c >> inID >> inRX >> inRY >> inRT;
            if (ISTR.fail()) {
                #ifdef _DEBUG
                std::cout << "DATABASE: Invalid message: " << buffer << std::endl;
                #endif
                continue;
            }
            ArPose inPO(inRX, inRY, inRT);

            if (inID==deviceID || inID=="all") { // it is us
                #ifdef _DEBUG
                std::cerr <<"DATABASE: We received goal/waypoint: " << buffer << std::endl;
                #endif
                if (KnownRobots[inID].located) {
                    // at this moment we do not calculate translation, assume that it is true zero
                    // ** translate inPO here **
                    // ** continuously attempt finding/updating translation in a separate thread **
                    if (buffer[0]=='G') {
                        robotGoal.setValue(inPO);
                    } else { /* buffer[0]=='W' */
                        robotWayPoint.setValue(inPO);
                    }
                } else {
                    // at this moment we do not calculate translation, assume that it is true zero
                    // ** translate inPO here **
                    // ** continuously attempt finding/updating translation in a separate thread **
                    if (buffer[0]=='G') {
                        robotGoal.setValue(inPO);
                    } else { /* buffer[0]=='W' */
                        robotWayPoint.setValue(inPO);
                    }
                }
                /* Example use:
                    robotGoal.lock();
                    double x = robotGoal.getValue().getX();
                    double y = robotGoal.getValue().getY();
                    double t = robotGoal.getValue().getTheta();
                    ...
                    ArPose p = robotGoal.getValue();
                    ...
                    robotGoal.unlock();
                */
            }
        } else if (buffer[0]=='M') {
            char c;
            std::string inID;
            unsigned short int inMode=0;
            std::istrstream ISTR(buffer);
            ISTR >> c >> inID >> inMode;
            if (ISTR.fail()) {
                #ifdef _DEBUG
                std::cout << "DATABASE: Invalid message: " << buffer << std::endl;
                #endif
                continue;
            }
            if (inID==deviceID || inID=="all") {               // it is us
                #ifdef _DEBUG
                std::cerr <<"DATABASE: We received mode: " << buffer << std::endl;
                #endif
                robotMode.setValue(static_cast<MagMode>(inMode));
            }
        } else if (buffer[0]=='J') {
            char c;
            std::string inID;
            double inX=0, inY=0, inZ=0, inR=0;
            unsigned short int inButtons = 0, inHat = 0;
            std::istrstream ISTR(buffer);
            ISTR >> c >> inID >> inX >> inY >> inZ >> inR >> inButtons >> inHat;
            if (ISTR.fail()) {
                #ifdef _DEBUG
                std::cout << "DATABASE: Invalid message: " << buffer << std::endl;
                #endif
                continue;
            }

            if (inID==deviceID) {               // it is us
                #ifdef _DEBUG
                std::cerr <<"DATABASE: We received joystick: " << buffer << std::endl;
                #endif
                robotJoystick.lock();
                robotJoystick.getValue().setState(inX, inY, inZ, inR, inButtons, inHat, CurrentTime());
                robotJoystick.unlock();
                /* Example use:
                    robotJoystick.lock();
                     if ( robotJoystick.getValue().getTS() - CurrentTime() < 60 ) {
                        double x = robotJoystick.getValue().getX();
                        ...
                     }
                    robotJoystick.unlock();
                */
            }
        } else {
            #ifdef _DEBUG
            std::cout << "DATABASE: Invalid message: " << buffer << std::endl;
            #endif
        }

    }
    #ifdef _DEBUG
    std::cerr <<"DATABASE: Exiting thread" << std::endl;
    #endif
}