#include "roboglue_shared.h"

RoboGlue_SHARED::RoboGlue_SHARED(QSettings *set){
    settings=set;

    robotVariables.insert(std::pair<std::string, int>("RDY",1));


}
////////////////////////////////////////////////////////////////
/////////////////////// RESTORE FUNCTIONS /////////////////////
/////////////////////////////////////////////////////////////
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();
}

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();
}

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();
}