#include "roboglue_shared.h"

RoboGlue_SHARED::RoboGlue_SHARED(QSettings *set){
    settings=set;
    //FIXME: inserire la lettura di quesste informazioni da un file
    robotVariables.insert(std::pair<std::string, int>("RDY",1));
    statusTimer = new QTimer(this);
    connect(statusTimer, SIGNAL(timeout()), this, SLOT(on_statusTimer()));
    statusTimer->start(static_cast<int>(comSettings.deadTime*1000));
}

RoboGlue_SHARED::~RoboGlue_SHARED(){
    statusTimer->stop();
    delete statusTimer;
}

void RoboGlue_SHARED::on_statusTimer(void){
    double curTime = static_cast<double>(std::time(NULL));
    commonStatus.relayTime = fabs(commonStatus.relayLast - curTime);
    commonStatus.followerTime = fabs(commonStatus.followerLast - curTime);
    commonStatus.recorderTime = fabs(commonStatus.recorderLast - curTime);
    emit this->commonStatusChange();
}
////////////////////////////////////////////////////////////////
/////////////////////// RESTORE FUNCTIONS /////////////////////
/////////////////////////////////////////////////////////////
void RoboGlue_SHARED::restoreCommonSettings(){
    settings->beginGroup("common");
    settings->endGroup();
}

void RoboGlue_SHARED::restoreInitSettings(){
    settings->beginGroup("init");
        initSettings.size=settings->value("size").toSize();
        initSettings.pos=settings->value("pos").toPoint();
        initSettings.autoMain=settings->value("automain").toBool();
        initSettings.autoCom=settings->value("autocom").toBool();
        initSettings.autoTrack=settings->value("autotrack").toBool();
        initSettings.autoGui=settings->value("autogui").toBool();
    settings->endGroup();
}

void RoboGlue_SHARED::restoreComSettings() {
    settings->beginGroup("robot");
        // connection parameters
        settings->beginGroup("connection");
            comSettings.connection.robotIp=settings->value("robotip").toString().toStdString();
            comSettings.connection.robotPort=settings->value("robotport").toUInt();
            comSettings.connection.retry=static_cast<uchar>(settings->value("robotretry").toUInt());
            comSettings.connection.autoConnect=settings->value("autoconnect").toBool();
            comSettings.connection.prefix=settings->value("prefix").toChar().toLatin1();
            comSettings.connection.suffix=settings->value("suffix").toChar().toLatin1();
            settings->endGroup();
        // kinematic parameters
        settings->beginGroup("kine");
            int ksize=settings->beginReadArray("dhtable");
            for(int i=0; i<ksize; i++){
                settings->setArrayIndex(i);
                comSettings.kine.DHtable.DHa[i] = settings->value("dha").toDouble();
                comSettings.kine.DHtable.DHd[i] = settings->value("dhd").toDouble();
                comSettings.kine.DHtable.DHalpha[i] = settings->value("dhaplha").toDouble();
                comSettings.kine.DHtable.DHtheta[i] = settings->value("dhtheta").toDouble();
            }
            settings->endArray();
            comSettings.kine.maxReach = settings->value("maxreach").toFloat();
       settings->endGroup();
       // exchange variables
       settings->beginGroup("variables");
            // TODO something
       settings->endGroup();
    settings->endGroup();
    settings->beginGroup("ros");
         comSettings.deadTime = settings->value("deadTime").toDouble();
         settings->beginGroup("ros_nodes");
             settings->beginGroup("mqtt_publishers");
                 comSettings.pubMap["commands"] = settings->value("commands").toString().toStdString();
                 comSettings.pubMap["coordinates"] = settings->value("coordinates").toString().toStdString();
                 comSettings.pubMap["state"] = settings->value("state").toString().toStdString();
                 comSettings.pubMap["interface"] = settings->value("interface").toString().toStdString();
             settings->endGroup();
             settings->beginGroup("mqtt_subscribers");
                 comSettings.subMap["commands"] = settings->value("commands").toString().toStdString();
                 comSettings.subMap["coordinates"] = settings->value("coordinates").toString().toStdString();
                 comSettings.subMap["state"] = settings->value("state").toString().toStdString();
                 comSettings.subMap["interface"] = settings->value("interface").toString().toStdString();
             settings->endGroup();
             settings->beginGroup("node_names");
                 comSettings.nodeNames["relay"] = settings->value("relay").toString().toStdString();
                 comSettings.nodeNames["follower"] = settings->value("follower").toString().toStdString();
                 comSettings.nodeNames["recorder"] = settings->value("recorder").toString().toStdString();
             settings->endGroup();
         settings->endGroup();
    settings->endGroup();
}

void RoboGlue_SHARED::restoreGuiSettings() {
    settings->beginGroup("gui");
        guiSettings.namePath=settings->value("namepath").toString().toStdString();
    settings->endGroup();
}

void RoboGlue_SHARED::restoreMainSettings() {
    settings->beginGroup("main");
    settings->endGroup();
}

void RoboGlue_SHARED::restoreTrackSettings() {
    settings->beginGroup("track");
    settings->endGroup();
}

////////////////////////////////////////////////////////////////
/////////////////////// SAVE FUNCTIONS ////////////////////////
//////////////////////////////////////////////////////////////
void RoboGlue_SHARED::saveCommonSettings(){
    settings->beginGroup("common");
    settings->endGroup();
}

void RoboGlue_SHARED::saveInitSettings() {
    settings->beginGroup("init");
        settings->setValue("autocom",initSettings.autoCom);
        settings->setValue("autogui",initSettings.autoGui);
        settings->setValue("autotrack",initSettings.autoTrack);
        settings->setValue("automain",initSettings.autoMain);
    settings->endGroup();
}

void RoboGlue_SHARED::saveComSettings() {
    settings->beginGroup("robot");
        settings->beginGroup("kine");
            settings->setValue("maxreach", comSettings.kine.maxReach);
            settings->setValue("prefix", comSettings.connection.prefix);
            settings->setValue("suffix", comSettings.connection.suffix);
        settings->endGroup();
    settings->endGroup();
    settings->beginGroup("ros");
        settings->setValue("deadTime", comSettings.deadTime);
        settings->beginGroup("ros_nodes");
            settings->beginGroup("mqtt_publishers");
                settings->setValue("commands", QString().fromStdString(comSettings.pubMap.find("commands")->second));
                settings->setValue("coordinates", QString().fromStdString(comSettings.pubMap.find("coordinates")->second));
                settings->setValue("state", QString().fromStdString(comSettings.pubMap.find("state")->second));
                settings->setValue("interface", QString().fromStdString(comSettings.pubMap.find("interface")->second));
            settings->endGroup();
            settings->beginGroup("mqtt_subscribers");
                settings->setValue("commands", QString().fromStdString(comSettings.subMap.find("commands")->second));
                settings->setValue("coordinates", QString().fromStdString(comSettings.subMap.find("coordinates")->second));
                settings->setValue("state", QString().fromStdString(comSettings.subMap.find("state")->second));
                settings->setValue("interface", QString().fromStdString(comSettings.subMap.find("interface")->second));
            settings->endGroup();
            settings->beginGroup("node_names");
                settings->setValue("relay", QString().fromStdString(comSettings.nodeNames.find("relay")->second));
                settings->setValue("follower", QString().fromStdString(comSettings.nodeNames.find("follower")->second));
                settings->setValue("recorder", QString().fromStdString(comSettings.nodeNames.find("recorder")->second));
            settings->endGroup();
        settings->endGroup();
    settings->endGroup();
}

void RoboGlue_SHARED::saveGuiSettings() {
    settings->beginGroup("gui");
        settings->setValue("namepath", QString(guiSettings.namePath.c_str()));
    settings->endGroup();
}

void RoboGlue_SHARED::saveMainSettings() {
    settings->beginGroup("main");
    settings->endGroup();
}

void RoboGlue_SHARED::saveTrackSettings() {
    settings->beginGroup("track");
    settings->endGroup();
}