#include "roboglue_init.h"
#include "ui_roboglue_init.h"

RoboGlue_INIT::RoboGlue_INIT(QWidget *parent, const char*) : QMainWindow(parent) {

    //////// SETUP LOGGER //////////
    inilog = spdlog::stdout_logger_mt("RoboGlue_inilog");
    inilog->info("INIT Started");

    //////// SETUP USER INTERFACE ///////
    ui = new Ui::RoboGlue_INIT;
    ui->setupUi(this);

    //////// INITIALIZE MEMORY MODULE ////////////
    QCoreApplication::setOrganizationName("ETsoft");
    QCoreApplication::setApplicationName("RoboGlue");
    settings = new QSettings("../RoboGlue/roboglue.conf", QSettings::IniFormat);
    sharedmem = new RoboGlue_SHARED(settings);

    //////// RESTORE SETTINGS FOR ALL MODULES ////////////
    sharedmem->restoreInitSettings();
    sharedmem->restoreComSettings();
    sharedmem->restoreGuiSettings();
    sharedmem->restoreMainSettings();
    sharedmem->restoreTrackSettings();

    //////// RESTORE UI STATE ////////////
    ui->chk_comAuto->setChecked(sharedmem->initSettings.autoCom);
    ui->chk_guiAuto->setChecked(sharedmem->initSettings.autoGui);
    ui->chk_mainAuto->setChecked(sharedmem->initSettings.autoMain);
    ui->chk_trackAuto->setChecked(sharedmem->initSettings.autoTrack);

    //////// AUTO START MODULES //////////
    if (sharedmem->initSettings.autoGui) this->on_btn_guiStart_clicked();
    if (sharedmem->initSettings.autoMain) this->on_btn_mainStart_clicked();
    if (sharedmem->initSettings.autoCom) this->on_btn_comStart_clicked();
    if (sharedmem->initSettings.autoTrack) this->on_btn_trackStart_clicked();

    //////// CONNECT SIGNALS & SLOTS ///////
    connect(this, SIGNAL(destroyed()), this, SLOT(deleteLater()), Qt::ConnectionType::QueuedConnection);
}

RoboGlue_INIT::~RoboGlue_INIT() {
    // close order must be opposite of opening one
    on_btn_trackStop_clicked();
    on_btn_comStop_clicked();
    on_btn_mainStop_clicked();
    on_btn_guistop_clicked();

    sharedmem->saveInitSettings();
    sharedmem->saveMainSettings();
    sharedmem->saveTrackSettings();
    sharedmem->saveComSettings();
    sharedmem->saveGuiSettings();

    delete robot;
    delete gui;
    delete main;
    delete track;
    delete settings;
    delete sharedmem;
    delete ui;
}


///////////////////////////////////////////////////
/////////////// HEARTBEAT SLOTS ///////////////////
///////////////////////////////////////////////////
void RoboGlue_INIT::on_robotHeartBeat(){

}
void RoboGlue_INIT::on_guiHeartBeat(){

}

void RoboGlue_INIT::on_mainHeartBeat(){

}

void RoboGlue_INIT::on_trackHeartBeat(){

}
///////////////////////////////////////////////////
/////////////// BUTTON SLOTS //////////////////////
///////////////////////////////////////////////////
void RoboGlue_INIT::on_btn_comStart_clicked(){
    if (robot == nullptr) {
        robot = new RoboGlue_COM(sharedmem);
        robot->start();
        // connect signals
        if (gui != nullptr){
            // robot to gui
            connect(robot, SIGNAL(com_newData(void)),
                    gui, SLOT(on_newRobotData(void)), Qt::ConnectionType::QueuedConnection);
            //gui to robot
            connect(gui, SIGNAL(sendROScommand(QString, QVariantMap)),
                    robot, SLOT(on_sendROScommand(QString,QVariantMap)),
                    Qt::ConnectionType::QueuedConnection);
            connect(gui, SIGNAL(sendROScoordinates(QList<double>)),
                    robot, SLOT(on_sendROScoordinates(QList<double>)),
                    Qt::ConnectionType::QueuedConnection);
            connect(gui, SIGNAL(sendROSstate(QMap<std::string, bool>, QMap<std::string, uint16_t>)),
                    robot, SLOT(on_sendROSstate(QMap<std::string, bool>, QMap<std::string, uint16_t>)),
                    Qt::ConnectionType::QueuedConnection);
            connect(gui, SIGNAL(robotSendURscript(QString)),
                    robot, SLOT(on_sendURscript(QString)),
                    Qt::ConnectionType::QueuedConnection);
            connect(gui, SIGNAL(robotConnect(QString, uint, uchar)),
                    robot, SLOT(on_connect(QString, uint, uchar)),
                    Qt::ConnectionType::QueuedConnection);
            connect(gui, SIGNAL(robotDisconnect()),
                    robot, SLOT(on_disconnect()),
                    Qt::ConnectionType::QueuedConnection);
        }
        // robot to init
        connect(robot, SIGNAL(com_heartBeat(void)),
                this, SLOT(on_robotHeartBeat(void)), Qt::ConnectionType::QueuedConnection);
        // shared memory to robot
        connect(sharedmem, SIGNAL(commonStatusChange(void)),
                robot, SLOT(on_commonStatusChange(void)));
    }
}

void RoboGlue_INIT::on_btn_comStop_clicked() {
    if (robot == nullptr) return;
    robot->on_quit();
    while (!robot->isFinished()){
        QThread::msleep(100);
    }
    robot->deleteLater();
    robot=nullptr;
}

void RoboGlue_INIT::on_btn_guiStart_clicked() {
    if (gui == nullptr) {
        gui = new RoboGlue_GUI(sharedmem);
        gui->show();
        // shared memory to robot
        connect(sharedmem, SIGNAL(commonStatusChange(void)),
                gui, SLOT(on_commonStatusChange(void)));
        if (robot != nullptr){
        }
    }
}

void RoboGlue_INIT::on_btn_guistop_clicked() {
    if (gui == nullptr) return;
    gui->deleteLater();
    gui = nullptr;
}

void RoboGlue_INIT::on_btn_mainStart_clicked() {
    if (main == nullptr){
        main = new RoboGlue_MAIN(sharedmem);
        main->start();
        // connect signals
        // connect()
    }
}

void RoboGlue_INIT::on_btn_mainStop_clicked(){
    if (main == nullptr) return;
    main->deleteLater();
    main = nullptr;
}

void RoboGlue_INIT::on_btn_trackStart_clicked() {
    if (track == nullptr){
        track = new RoboGlue_TRACK(sharedmem);
        track->start();
        // connect signals
        // connect()
    }
}

void RoboGlue_INIT::on_btn_trackStop_clicked() {
    if (track == nullptr) return;
    track->deleteLater();
    track = nullptr;
}

void RoboGlue_INIT::on_chk_comAuto_clicked(bool checked) {
    sharedmem->initSettings.autoCom=checked;
}

void RoboGlue_INIT::on_chk_guiAuto_clicked(bool checked) {
    sharedmem->initSettings.autoGui=checked;
}

void RoboGlue_INIT::on_chk_trackAuto_clicked(bool checked) {
    sharedmem->initSettings.autoTrack=checked;
}

void RoboGlue_INIT::on_chk_mainAuto_clicked(bool checked){
    sharedmem->initSettings.autoMain=checked;
}