#include "mag_variables.h"
#include "tnplib.h"                     // include BU-ECE Multiplatform Network Library
#include <iostream>

AutoSynchronized<int>   needtorestart(0);
std::string             deviceID = "";

ArMutex                 KnownRobotsLock;
std::map<std::string, RobotState> KnownRobots;

NAV_Robot               Robot("localhost", 8101);
NAV_Map                 MAP_G(100);
NAV_Map                 MAP_L(100);
std::string             MAP_G_fname = "map_global.ppm";
std::string             MAP_L_fname = "map_local.ppm";  // to be overwritten in the main initialization


SOCKET                  mcastsocket=0;
sockaddr_in             mcastaddr;
const int               MPORT = 60001;
const char*             MADDR = "239.0.0.77";

int NetComStart() {
    SocketLibStart();
    mcastsocket = UDPStartMServer(MPORT, 1);
    UDPMulticastJoin(mcastsocket , MADDR);
    UDPMulticastSetTTL(mcastsocket, 16);
    mcastaddr=CreateAddress(MADDR, MPORT);
    UDPSetTimeout(mcastsocket, 100);
    return(mcastsocket==0);
}

int NetComStop() {
    UDPMulticastDrop(mcastsocket , MADDR);
    UDPStopServer(mcastsocket);
    mcastsocket=0;
    SocketLibEnd();
    return(0);
 }

int NetComSend(const char *message) {
    return(UDPSendAny(mcastsocket, message, int(strlen(message)+1), mcastaddr));
}

int NetComRecv(char *message, int maxlen) {
    sockaddr_in senderAddr;
    return(UDPRecvAny(mcastsocket, message, maxlen, senderAddr));
}



#include <ctime>

long int CurrentTime() {
    std::time_t now;
    std::time( &now );
    return(static_cast<long int>(now));
}

void InitRobot() {
    std::time_t now;
    std::time( &now );
    std::srand(static_cast<unsigned int>(now));

    do {
        std::cout << "Please enter the agent ID name (/[A-Za-z0-9)]{3,}/) ";
        std::getline(std::cin, deviceID);
    } while (deviceID.size()<3 || deviceID.find_first_of(" \t?*.")!=std::string::npos); 
            // at least 3 characters    and     no white space inside

    MAP_L_fname = "map_local_"+deviceID+".ppm";

    Robot.moveCoords(ArPose(0,0,0));

    KnownRobotsLock.lock();
    // set entry for our robot -- located transformation position timestamp
    KnownRobots[deviceID] = RobotState(true, ArPose(0,0,0), ArPose(0,0,0), now);
    KnownRobotsLock.unlock();
}