#include "roboglue_com.h"

RoboGlue_COM::RoboGlue_COM (RoboGlue_SHARED *mem): m(mem) {

    //////// SETUP LOGGER //////////
    liblog = spdlog::stdout_logger_mt("URcom_liblog");
    modlog = spdlog::stdout_logger_mt("RoboGlue_comlog");
    liblog->set_level(spdlog::level::info);
    modlog->set_level(spdlog::level::debug);

    ///////// INITIALIZE TCP SERVER ///////////
    server = new QTcpServer(this);
    server->listen(QHostAddress::Any, 9000);
    connect(server, SIGNAL(newConnection()), this, SLOT(on_internalRobotConnect()));
    modlog->info("COM started");

    //////// INITIALIZE MQTT CONNECTOR ///////
    client = new mqtt::async_client(std::string("tcp://localhost:1883"),"");
    callback = new mqtt_callback(client,modlog);////////////////////////////////////////////////
    //////// END EXTERNAL PUBLIC SLOTS /////////////
    ////////////////////////////////////////////////
    baseMsg = m->commonSettings.baseMessage;
    baseMsg["command"]=nullptr;
    baseMsg["params"]=nullptr;

    //////// INITIALIZE TIMED CALLBACK FUNCTIONS //////
    coordSendTimer = new QTimer(this);
    connect(coordSendTimer, SIGNAL(timeout()), this, SLOT(timed_sendCoordinates()));
}

void RoboGlue_COM::run() {
    unsigned char attempt = 0;

    /// START MQTT CLIENT ////
    client->set_callback(*callback);
    client->connect()->wait();
    // subscrbe to mqtt topics
    std::vector<std::string> subList = m->comSettings.subList;
    for (uint i=0; i < subList.size(); i++){
        client->subscribe(subList[i],0);
    }
    client->start_consuming();

    /// AUTOCONNECT ROBOT INTERFACE ///
    if(m->comSettings.connection.autoConnect) this->on_connect(QString(m->comSettings.connection.robotIp.c_str()),
                                                               m->comSettings.connection.robotPort,
                                                               m->comSettings.connection.retry);
    /// THREAD LOOP ///
    while(this->isRunning){
        // if host is not connected and the reader thread is stopped do nothing
        if (!this->isConnected & !this->isReading) {
            msleep(100);
        }
        // if host is connected but reader thread is not started, start it
        else if (this->isConnected & !this->isReading) {
            this->isConnected=false;
            // create new robot interface
            interface = new UR_CLinterface(liblog);
            // try connect to robot
            while (!this->isConnected & (attempt++ < m->comSettings.connection.retry)){
                attempt = 0;
                modlog->debug("Connection Attempt:{0}",attempt);
                try {
                this->isConnected = interface->connect(m->comSettings.connection.robotIp.c_str(),
                                                       m->comSettings.connection.robotPort);
                } catch (boost::system::system_error &e){
                    modlog->error("Connection Failed:\n {}", e.what());
                }
            }
            if (this->isConnected & ! this->isReading) {
                // create new reader thread and connect new data signal
                modlog->info("Interface Connected");
                readerThr = new readerThread(&this->isReading, interface, &localData);
                // connect readerThread newdata signal
                connect(readerThr, SIGNAL(newDataAvailable()), this, SLOT(on_internalNewData()));
                readerThr->start();
                modlog->debug("ReaderThread is Working");
                this->isReading = true;
            } else modlog->error("Connection failed");
        }
        // if host is connected and reader is running, do nothing
        else if (this->isConnected & this->isReading) {
            if (robotConnected){
                if(robotIN->bytesAvailable()){
                    robotRequest = robotIN->readAll().toStdString();
                    robotRequest.pop_back();
                    modlog->info("robotMessage {}", robotRequest.c_str());
                    processRobotRequest(robotRequest);
                }
            }
            emit com_heartBeat();
            msleep(10);
        }
        // if host is to be disconnected but still reading, stop reading and disconnect host
        else if (!this->isConnected & this->isReading){
            // disable reading, should stop reader thread, disconnect new data signal
            disconnect(readerThr, SIGNAL(newDataAvailable()), this, SLOT(on_internalNewData()));
            this->isReading = false;
            readerThr->wait();
            modlog->debug("ReaderThread is Stopped");
            // wait until thread exits
            this->isConnected=false;
            interface->disconnect();
            modlog->info("Interface Disconnected");
            // schedule readerThread for deletion;
            readerThr->deleteLater();
            // delete and rebuild interface for next connection;
            delete interface;
            interface = nullptr;
        }
    }
    if (isConnected) {
        robotIN->disconnectFromHost();
        robotIN->deleteLater();
        server->deleteLater();
        on_disconnect();
        readerThr->wait();
        coordSendTimer->stop();
    }
    if (interface != nullptr) delete interface;
    if (coordSendTimer != nullptr) delete coordSendTimer;
    spdlog::drop("URcom_liblog");
    spdlog::drop("RoboGlue_comlog");
}


/////////////////////////////////////////////////
//////////////////// READER THREAD //////////////
////////////////////////////////////////////////
void readerThread::run(){
    this->loopRead();
}
void readerThread::loopRead() {
    while(*this->isReading){
        interface->readData();
        interface->parseData();
        *dataOut=interface->clientData;
        // emit signal every new data received
        emit newDataAvailable();
    }
}
/////////////////////////////////////////////////
///////////////END READER THREAD////////////////
////////////////////////////////////////////////

////////////////////////////////////////////////
/////////////// MQTT FUNCTIONS /////////////////
////////////////////////////////////////////////
void RoboGlue_COM::MQTTpublish(json msg, string topic) {
    std::string msgstr = msg.dump();
    mqtt::token_ptr tok = client->publish(mqtt::make_message(topic,msgstr));
    modlog->trace("Published: id:{0}, topic:{1}, msg:{2}",
                  tok->get_message_id(), tok->get_topics(), msgstr);
}
////////////////////////////////////////////////
////////////END MQTT FUNCTIONS /////////////////
////////////////////////////////////////////////

void RoboGlue_COM::saveKinematicInfo() {
    m->mutex.lock();
    m->settings->beginGroup("robot");
        m->settings->beginGroup("kine");
            m->settings->beginWriteArray("dhtable");
            for (int i=0; i<JOINT_NUMBER; i++) {
                m->settings->setArrayIndex(i);
                m->settings->setValue("dha", localData.configuration.kinematics.DHa[i]);
                m->settings->setValue("dhd", localData.configuration.kinematics.DHd[i]);
                m->settings->setValue("dhalpha", localData.configuration.kinematics.DHalpha[i]);
                m->settings->setValue("dhtheta", localData.configuration.kinematics.DHtheta[i]);
            }
            m->settings->endArray();
        m->settings->endGroup();
    m->settings->endGroup();
    m->mutex.unlock();
}

void RoboGlue_COM::saveConnectionInfo() {
    m->mutex.lock();
    m->settings->beginGroup("robot");
        m->settings->beginGroup("connection");
            m->settings->setValue("robotip",QString(m->comSettings.connection.robotIp.c_str()));
            m->settings->setValue("robotport",m->comSettings.connection.robotPort);
            m->settings->setValue("robotretry",m->comSettings.connection.retry);
            m->settings->setValue("autoconnect",m->comSettings.connection.autoConnect);
        m->settings->endGroup();
    m->settings->endGroup();
    m->mutex.unlock();
}

void RoboGlue_COM::processRobotRequest(std::string request) {
    std::string response;
    std::string varName;
    std::string varValue;
    std::vector<std::string> tokes;

    m->mutex.lock();
    // split command into tokes
    boost::algorithm::split(tokes,request,boost::is_any_of(" "));
    // process requested action
    if (tokes[COMMAND] == "GET"){
        modlog->debug("Robot GET Request");
        varName = tokes[VARNAME];
        response = varName + " ";
        try {
            response += std::to_string(m->robotVariables.at(varName));
        } catch (std::out_of_range &e) {
            modlog->error("{0}:{1}",e.what(), varName);
        }
        response.append("\n");
    } else
    if (tokes[COMMAND] == "SET") {
        modlog->debug("Robot SET Request");
        varName = tokes[VARNAME];
        varValue = tokes[VALUE];
        try {
            // if variable does not exist insert it into map
            m->robotVariables[varName] = std::stoi(varValue.c_str());
        } catch (std::out_of_range &e) {
            modlog->error("{0}:{1}",e.what(), varName);
        }
    } else {
        modlog->debug("Robot GENERAL Request");
        response = "generalrequest";
        response.insert(0,&m->comSettings.connection.prefix);
        response.append(&m->comSettings.connection.suffix);
    }
    m->mutex.unlock();
    robotIN->write(response.c_str());
    robotIN->flush();
}

////////////////////////////////////////////////
////////////INTERNAL PRIVATE SLOTS//////////////
////////////////////////////////////////////////
void RoboGlue_COM::on_internalRobotConnect() {
    if(robotIN != nullptr) {
        robotConnected = false;
        delete robotIN;
    }
    modlog->info("Robot request connected");
    robotIN = server->nextPendingConnection();
    robotConnected = true;
}

void RoboGlue_COM::on_internalNewData(){
    this->m->mutex.lock();
    m->robotData=this->localData;
    this->m->mutex.unlock();
    emit com_newData();
    // save DH matrix data;
    if(isFirstMessage) {
        this->saveKinematicInfo();
        isFirstMessage = false;
    }
}

void RoboGlue_COM::timed_sendCoordinates(){
    QList<double> sp = this->lastCoord;
    if (this->isCoordNew)
        this->isCoordNew = false;
    if (!sp.empty()){
        modlog->trace("timedSendCoordinate");
        baseMsg.clear();
        baseMsg["type"] = "cartesian";
        baseMsg["mode"] = "pos";
        baseMsg["value"] = json();
        baseMsg["value"]["x"] = sp[0];
        baseMsg["value"]["y"] = sp[1];
        baseMsg["value"]["z"] = sp[2];
        baseMsg["value"]["rx"] = sp[3];
        baseMsg["value"]["ry"] = sp[4];
        baseMsg["value"]["rz"] = sp[5];
        MQTTpublish(baseMsg, m->comSettings.pubList[m->comSettings.COORD]);
    }
}
////////////////////////////////////////////////
////////END INTERNAL PRIVATE SLOTS//////////////
////////////////////////////////////////////////

void RoboGlue_COM::on_commonStatusChange() {
    if (m->commonStatus.isRecording || m->commonStatus.isRealtime) {
        if (!coordSendTimer->isActive()) coordSendTimer->start(100);
    }
    if (!m->commonStatus.isRecording && !m->commonStatus.isRealtime){
        if (coordSendTimer->isActive()) coordSendTimer->stop();
    }
}

void RoboGlue_COM::on_quit() {
    this->isConnected=false;
    client->disconnect();
    while(this->isReading) msleep(100);
    this->isRunning = false;
}

void RoboGlue_COM::on_connect(QString ip, uint port, uchar retry) {
    modlog->debug("robotConnect Received");
    if (!isConnected){
        this->isConnected=true;
        this->isReading=false;
        this->isFirstMessage=true;
        // update connection information for the thread
        m->comSettings.connection.robotIp = ip.toStdString();
        m->comSettings.connection.robotPort = port;
        m->comSettings.connection.retry = retry;
        // save connection info
        saveConnectionInfo();
    }
}

void RoboGlue_COM::on_disconnect() {
    modlog->debug("robotDisconnect Received");
    if(isConnected){
        this->isConnected = false;
    }
}

void RoboGlue_COM::on_sendURscript(QString command) {
    modlog->debug("robotSendURscript Received");
    if(isConnected){
        //command sent directly trough interface, NOT ROS
        this->interface->sendCommand(command.toStdString());
    }
}

void RoboGlue_COM::on_sendROScommand(QString command, QVariantMap params) {
    modlog->debug("robotSendCommand Received");
    baseMsg.clear();
    baseMsg["command"] = command.toStdString();
    QJsonDocument jparam = QJsonDocument(QJsonObject::fromVariantMap(params));
    baseMsg["params"] = json::parse(jparam.toJson().data());
    MQTTpublish(baseMsg, m->comSettings.pubList[m->comSettings.COMM]);
}

void RoboGlue_COM::on_sendROScoordinates(QList<double> sp) {
    this->lastCoord.clear();
    this->lastCoord = sp;
    this->isCoordNew = true;
}

void RoboGlue_COM::on_sendROSstate(QMap<std::string,bool> digital, QMap<std::string, uint16_t> analog){
    modlog->debug("robotSendState Received");
    baseMsg.clear();
    baseMsg["digital"] = nlohmann::json();
    baseMsg["analog"] = nlohmann::json();
    for (auto di : digital.keys()) {
        baseMsg["digital"][di] = digital[di];
    }
}
////////////////////////////////////////////////
//////// END EXTERNAL PUBLIC SLOTS /////////////
////////////////////////////////////////////////
//void RoboGlue_COM::on_sendSetpointOLD(QList<double> sp) {
//    std::string setpoint;
//    struct {
//        union{
//            struct{
//                uint32_t x,y,z,a,b,c,keepalive;
//            };
//            char bytes[28];
//        };
//    } setPointBytes;
//    setPointBytes.keepalive=1;
//    int32_t v;
//    setpoint = "(";
//    for(uchar i=0; i<JOINT_NUMBER; i++){
//        v=static_cast<int32_t>(sp[i]*1000);
//        if (i>2) v=boost::algorithm::clamp(v, -2680,2680);
//        setpoint.append(std::to_string(v));
//        boost::algorithm::replace_all(setpoint, ",",".");
//        if(i<JOINT_NUMBER) setpoint.append("|");
//    }
//    setpoint.append("1");
//    boost::algorithm::replace_all(setpoint,"|",",");
//    setpoint.append(")");

//    setPointBytes.x=htonl(static_cast<uint32_t>(sp[0]*1000));
//    setPointBytes.y=htonl(static_cast<uint32_t>(sp[1]*1000));
//    setPointBytes.z=htonl(static_cast<uint32_t>(sp[2]*1000));
//    setPointBytes.a=htonl(static_cast<uint32_t>(sp[3]*1000));
//    setPointBytes.b=htonl(static_cast<uint32_t>(sp[4]*1000));
//    setPointBytes.c=htonl(static_cast<uint32_t>(sp[5]*1000));
//    setPointBytes.keepalive=htonl(setPointBytes.keepalive);

//    if(robotIN->write(setPointBytes.bytes,28))
//        modlog->trace("Sent Setpoint {}",setpoint.c_str());
//    robotIN->flush();
//}