#include "roboglue_gui.h"
#include "ui_roboglue_gui.h"

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

    //////// SETUP LOGGER //////////
    modlog = spdlog::stdout_logger_mt("RoboGlue_guilog");
    spdlog::set_level(spdlog::level::debug);
    modlog->info("GUI Started");

    //////// SETUP USER INTERFACE ///////
    ui=new Ui::RoboGlue_GUI;
    ui->setupUi(this);
    ui->frm_move->setStyleSheet("border-image: url(:/images/PiantaRobot_Alto.png) 0 0 0 0 stretch stretch");

    //////// RESTORE UI STATE  ////////////
    ui->txt_roboIp->setText(QString(m->comSettings.connection.robotIp.c_str()));
    ui->txt_robotPort->setText(QString::number(m->comSettings.connection.robotPort));
    ui->chk_autoConnect->setChecked(m->comSettings.connection.autoConnect);

    //////// ENABLE HOVERING EVENT OVER DEFINED AREA //////////////
    ui->frm_move->installEventFilter(this);

    //////// CONNECT SIGNALS & SLOTS ///////
    connect(this, SIGNAL(destroyed()),
            this, SLOT(deleteLater()), Qt::ConnectionType::QueuedConnection);
    connect(this, SIGNAL(pad_hoverEvent(QEvent *)),
            this, SLOT(on_pad_hoverEvent(QEvent *)));
    connect(m, SIGNAL(commonStatusChange()),
            this, SLOT(on_commonStatusChange()));

    /////// READ NAME DEFINITION JSON FILE //////////
    try {
        QFile fp(QString(m->guiSettings.namePath.c_str()));
        fp.open(QFile::ReadOnly | QFile::Text);
        QByteArray rawdata= fp.readAll();
        std::string strdata= rawdata.toStdString();
        nameDefinitions = json::parse(strdata.c_str());
        fp.close();
    } catch (json::exception e){
        modlog->error("Unable to parse name definitions:\n\t{}",e.what());
    }
}

RoboGlue_GUI::~RoboGlue_GUI() {
    spdlog::drop("RoboGlue_guilog");
    delete ui;
}

bool RoboGlue_GUI::eventFilter(QObject *obj, QEvent *event){
    if(obj == static_cast<QObject *>(ui->frm_move))
        if(event->type()==event->MouseMove || event->type()==event->MouseButtonPress
                || event->type()==event->KeyPress){
            event->accept();
            emit pad_hoverEvent(event);
        }
    return QWidget::eventFilter(obj, event);
}

QVariantMap RoboGlue_GUI::getLockAxes() {
    QVariantMap param;
    param["lx"]=ui->chk_lockX->isChecked();
    param["ly"]=ui->chK_lockY->isChecked();
    param["lz"]=ui->chk_lockZ->isChecked();
    param["lrx"]=ui->chk_lockRX->isChecked();
    param["lry"]=ui->chk_lockRY->isChecked();
    param["lrz"]=ui->chk_lockRZ->isChecked();
    return param;
}

void RoboGlue_GUI::enableLockAxes() {
    ui->chk_lockX->setEnabled(true);
    ui->chK_lockY->setEnabled(true);
    ui->chk_lockZ->setEnabled(true);
    ui->chk_lockRX->setEnabled(true);
    ui->chk_lockRY->setEnabled(true);
    ui->chk_lockRZ->setEnabled(true);
}

void RoboGlue_GUI::disableLockAxes(){
    ui->chk_lockX->setEnabled(false);
    ui->chK_lockY->setEnabled(false);
    ui->chk_lockZ->setEnabled(false);
    ui->chk_lockRX->setEnabled(false);
    ui->chk_lockRY->setEnabled(false);
    ui->chk_lockRZ->setEnabled(false);
}

////////////////////////////////////////////////
////////////INTERNAL PRIVATE SLOTS//////////////
////////////////////////////////////////////////
void RoboGlue_GUI::on_commonStatusChange() {
    modlog->trace("on_commonStatusChange Received");
    this->ui->lbl_relay->setText(QString().fromStdString(this->m->comSettings.nodeNames.find("relay")->second+": "+std::to_string(static_cast<uint>(m->commonStatus.relayTime))));
    this->ui->lbl_follower->setText(QString().fromStdString(this->m->comSettings.nodeNames.find("follower")->second+": "+std::to_string(static_cast<uint>(m->commonStatus.followerTime))));
    this->ui->lbl_recorder->setText(QString().fromStdString(this->m->comSettings.nodeNames.find("recorder")->second+": "+std::to_string(static_cast<uint>(m->commonStatus.recorderTime))));
    this->ui->lbl_broadcaster->setText(QString().fromStdString(this->m->comSettings.nodeNames.find("broadcaster")->second+": "+std::to_string(static_cast<uint>(m->commonStatus.broadcasterTime))));
    if (!m->commonStatus.relayRunning) this->ui->lbl_relay->setStyleSheet("QLabel { background-color : red; color : white; }");
    else this->ui->lbl_relay->setStyleSheet("QLabel { background-color : green; color : white; }");
    if (!m->commonStatus.recorderRunning) this->ui->lbl_recorder->setStyleSheet("QLabel { background-color : red; color : white; }");
    else this->ui->lbl_recorder->setStyleSheet("QLabel { background-color : green; color : white; }");
    if (!m->commonStatus.followerRunning) this->ui->lbl_follower->setStyleSheet("QLabel { background-color : red; color : white; }");
    else this->ui->lbl_follower->setStyleSheet("QLabel { background-color : green; color : white; }");
    if (!m->commonStatus.broadcasterRunning) this->ui->lbl_broadcaster->setStyleSheet("QLabel { background-color : red; color : white; }");
    else this->ui->lbl_broadcaster->setStyleSheet("QLabel { background-color : green; color : white; }");
}

void RoboGlue_GUI::on_pad_hoverEvent(QEvent* e) {
    QMouseEvent *mouse = static_cast<QMouseEvent *>(e);
    QKeyEvent *key = static_cast<QKeyEvent *>(e);
    static float zpos=0.5;
    static bool trackEnable = false;
    static QPointF lastPos;
    QPointF p;
    float limit = m->comSettings.kine.maxReach;

    modlog->trace("event: {}",e->type());
    if(e->type() == e->MouseMove){
        QPointF pos = mouse->localPos();
        qreal x = boost::algorithm::clamp(pos.x(),0,ui->frm_move->size().width());
        qreal y = boost::algorithm::clamp(pos.y(),0,ui->frm_move->size().height());
        p = QPointF(x,y);
    } else if(e->type() == e->MouseButtonPress &&
              (m->commonStatus.isRecording || m->commonStatus.isRealtime)) {
        modlog->trace("mousebuttonevent {}", mouse->button());
        if(!trackEnable){
            trackEnable=true;
            //mouse->setLocalPos(lastPos);
            ui->frm_move->setFocus();
            if (ui->btn_record->isEnabled()) ui->btn_record->setEnabled(false);
            if (ui->btn_realtime->isEnabled()) ui->btn_realtime->setEnabled(false);
        } else if(trackEnable){
            trackEnable=false;
            lastPos=mouse->pos();
            if (m->commonStatus.isRecording) ui->btn_record->setEnabled(true);
            if (m->commonStatus.isRealtime) ui->btn_realtime->setEnabled(true);
        }
    } else if (e->type()==e->KeyPress) {
        modlog->trace("keyevent {}", key->key());
        if(key->key() == 16777235)
            if (zpos < limit) zpos=zpos+0.1;
        if(key->key() == 16777237)
            if (zpos > 0) zpos=zpos-0.1;
    }

    if(trackEnable && (m->robotVariables.at("RDY") == 1)) {
        QTransform t=QTransform::fromTranslate(-ui->frm_move->size().width()/2,
                                               -ui->frm_move->size().height()/2);
        t = t * QTransform::fromScale(-limit/t.dx(),
                                     -limit/t.dy());
        QPointF q = t.map(p);
        ui->lbl_posX->setText(QString("X: %1m").arg(q.x(),6));
        ui->lbl_posY->setText(QString("Y: %1m").arg(-q.y(),6));
        ui->lbl_posZ->setText(QString("Z: %1m").arg(zpos,6));

        // FIXME
        ///////////////////////////////////////////////////////
        ////////////////// TEMPORARY SEND COORDINATES ////////
        //////////////////  TO ROBOT /////////////////////////
        /// (TO BE IMPLEMENTED IN MAIN FOR KINEMATICKS CHECK)
        if (q.x()>-limit && q.x()<limit && q.y()>-limit && q.y()<limit && zpos > 0 && zpos < limit){
            QList<double> sp = {q.x(),-q.y(),zpos,1.57,-2.68,0};
            emit sendROScoordinates(sp);
        }
    }
}

void RoboGlue_GUI::on_btn_sendCommand_clicked(){
    modlog->debug("robotSendCommand Requested");
    emit robotSendURscript(ui->txt_commandToSend->text());
}


void RoboGlue_GUI::on_btn_robotConnect_clicked() {
    modlog->debug("robotConnect Requested");
    emit robotConnect(ui->txt_roboIp->text(),
                      ui->txt_robotPort->text().toUInt(),
                      5); // default retry attempt to 5
}

void RoboGlue_GUI::on_btn_robotDisconnect_clicked() {
    modlog->debug("robotDisconnect Requested");
    emit robotDisconnect();
}

void RoboGlue_GUI::on_chk_autoConnect_clicked(bool checked) {
   m->comSettings.connection.autoConnect = checked;
   m->settings->beginGroup("robot");
        m->settings->beginGroup("connection");
            m->settings->setValue("autoconnect", checked);
        m->settings->endGroup();
        m->settings->endGroup();
}

void RoboGlue_GUI::on_btn_record_clicked() {
    static bool recording = false;
    QVariantMap param;
    QVariantMap metadata;
    modlog->debug("robotTrajectory Record");
    param.insert("action", "");
    if (!recording) {
        m->commonStatus.isRecording = true;
        recording = true;
        param["action"] = "start";
        param["lock"] = getLockAxes();
        metadata["name"] = ui->txt_fileName->text();
        metadata["code"] = QUuid::createUuid().toString();
        metadata["ds"] = ui->spn_ds->value();
        metadata["dt"] = ui->spn_dt->value();
        param["metadata"] = metadata;
        ui->btn_play->setEnabled(false);
        ui->btn_realtime->setEnabled(false);
        ui->btn_home->setEnabled(false);
        disableLockAxes();
    } else {
        m->commonStatus.isRecording = false;
        recording = false;
        param["action"]="stop";
        ui->btn_play->setEnabled(true);
        ui->btn_realtime->setEnabled(true);
        ui->btn_home->setEnabled(true);
        enableLockAxes();
    }
    emit m->commonStatusChange();
    emit sendROScommand("RECORD", param);
}

void RoboGlue_GUI::on_btn_play_clicked() {
    static bool playing = false;
    QVariantMap param;
    QVariantMap metadata;
    modlog->debug("robotTrajectory Replay");
    if (!playing) {
        m->commonStatus.isPlaying = true;
        playing = true;
        metadata["name"]=ui->txt_fileName->text();
        param["action"]="start";
        param["metadata"] = metadata;
        param["lock"] = getLockAxes();
        ui->btn_record->setEnabled(false);
        ui->btn_realtime->setEnabled(false);
        ui->btn_home->setEnabled(false);
        ui->btn_play->setText("Stop");
        ui->txt_fileName->setEnabled(false);
        disableLockAxes();
    } else {
        m->commonStatus.isPlaying = false;
        playing = false;
        param["action"] = "stop";
        ui->btn_record->setEnabled(true);
        ui->btn_realtime->setEnabled(true);
        ui->btn_home->setEnabled(true);
        ui->btn_play->setText("Play");
        ui->txt_fileName->setEnabled(true);
        enableLockAxes();
    }
    emit m->commonStatusChange();
    emit sendROScommand("PLAY", param);
}

void RoboGlue_GUI::on_btn_open_clicked() {
    static bool fileOpen = false;
    QVariantMap param;
    QVariantMap metadata;
    modlog->debug("robotTragectory Open");
    if(!fileOpen){
        m->commonStatus.isFileOpen = true;
        fileOpen = true;
        metadata["name"] = ui->txt_fileName->text();
        metadata["plot"] = ui->chk_plot->isChecked();
        param["action"] = "open";
        param["metadata"] = metadata;
        ui->btn_record->setEnabled(false);
        ui->btn_realtime->setEnabled(false);
        ui->btn_home->setEnabled(false);
        ui->btn_open->setText("Close");
        ui->txt_fileName->setEnabled(false);
    } else {
        m->commonStatus.isFileOpen = false;
        fileOpen = false;
        param["action"] = "close";
        ui->btn_record->setEnabled(true);
        ui->btn_realtime->setEnabled(true);
        ui->btn_home->setEnabled(true);
        ui->btn_open->setText("Open");
        ui->txt_fileName->setEnabled(true);
    }
    emit m->commonStatusChange();
    emit sendROScommand("OPEN", param);
}

void RoboGlue_GUI::on_btn_realtime_clicked() {
    static bool realtime = false;
    QVariantMap param;
    QVariantMap lock;
    modlog->debug("robotTrajectory Realtime");
    if (!realtime){
        realtime = true;
        m->commonStatus.isRealtime = true;
        param["action"] = "start";
        param["lock"] = getLockAxes();
        if (ui->rad_pos->isChecked()) param["mode"] = "pos";
        else if (ui->rad_vel->isChecked()) param["mode"] = "vel";
        ui->btn_play->setEnabled(false);
        ui->btn_record->setEnabled(false);
        disableLockAxes();
    } else {
        m->commonStatus.isRealtime = false;
        realtime = false;
        param["action"]="stop";
        ui->btn_play->setEnabled(true);
        ui->btn_record->setEnabled(true);
        enableLockAxes();
    }
    emit m->commonStatusChange();
    emit sendROScommand("REALTIME", param);
}

void RoboGlue_GUI::on_btn_home_clicked() {
    QVariantMap param;
    modlog->debug("robotHome");
    param["action"] = "start";
    param["execute"] = ui->chk_execute->isChecked();
    emit m->commonStatusChange();
    emit sendROScommand("HOME", param);
}

void RoboGlue_GUI::on_btn_setFrame_clicked() {
    QVariantMap param, frm;
    QList<QString> tempPos, TempOr;
    modlog->debug("setFrame");
    param["action"]="set";
    param["framename"]=ui->txt_frameName->text();
    for(auto tp : ui->txt_framePos->text().split(",")){
        frm[tp.split(":").first()]=tp.split(":").last().replace(" ", "").toDouble();
    }
    for(auto tp : ui->txt_frameOr->text().split(",")){
        frm[tp.split(":").first()]=tp.split(":").last().replace(" ", "").toDouble();
    }
    param["frame"]=frm;
    emit m->commonStatusChange();
    emit sendROScommand("SETFRAME", param);
}

void RoboGlue_GUI::on_btn_stopRos_clicked() {
    QVariantMap param;
    modlog->debug("stopRos");
    param["action"]=false;
    emit m->commonStatusChange();
    emit sendROScommand("QUIT", param);
}
////////////////////////////////////////////////
////////END INTERNAL PRIVATE SLOTS//////////////
////////////////////////////////////////////////

////////////////////////////////////////////////
////////////EXTERNAL PUBLIC SLOTS///////////////
////////////////////////////////////////////////
void RoboGlue_GUI::on_newRobotData() {
    m->mutex.lock();
        robotModeData_t mode = m->robotData.robotMode;
        cartesianInfo_t cart = m->robotData.cartesianInfo;
        jointData_t joint = m->robotData.jointData;
    m->mutex.unlock();

     //////////// mode info //////////
    try {
        ui->lbl_connected->setText(QString::number(mode.isRealRobotConnected));
        ui->lbl_robotMode->setText(QString(nameDefinitions["robotModes"][std::to_string(mode.robotMode).c_str()].get<std::string>().c_str()));
        ui->lbl_powerOn->setText(QString::number(mode.isRobotPowerOn));
        ui->lbl_emeStop->setText(QString::number(mode.isEmergencyStopped));
        ui->lbl_proStop->setText(QString::number(mode.isProtectiveStopped));
        ui->lbl_programRunning->setText(QString::number(mode.isProgramRunning));
        ui->lbl_controlMode->setText(QString(nameDefinitions["controlModes"][std::to_string(mode.controlMode).c_str()].get<std::string>().c_str()));
    } catch (json::exception e){
        modlog->error("Unable to find name: \n{}",e.what());
    }
    //////////// cartesian info //////////
    ui->lbl_cartX->setText("X: "+QString::number(cart.cartPosition.X*1000)+QString(" mm"));
    ui->lbl_cartY->setText("Y: "+QString::number(cart.cartPosition.Y*1000)+QString(" mm"));
    ui->lbl_cartZ->setText("Z: "+QString::number(cart.cartPosition.Z*1000)+QString(" mm"));
    ui->lbl_cartRX->setText("Rx: "+QString::number(cart.cartPosition.RX,'g',3)+QString(" rad"));
    ui->lbl_cartRY->setText("Ry: "+QString::number(cart.cartPosition.RY,'g',3)+QString(" rad"));
    ui->lbl_cartRZ->setText("Rz: "+QString::number(cart.cartPosition.RZ,'g',3)+QString(" rad"));
    //////////// joint info //////////
    ui->lbl_J1->setText("Base: " + QString::number(joint.jointParam[0].actual)+" rad");
    ui->lbl_J2->setText("Shoulder: " + QString::number(joint.jointParam[1].actual)+" rad");
    ui->lbl_J3->setText("Elbow: " + QString::number(joint.jointParam[2].actual)+" rad");
    ui->lbl_J4->setText("Wrist1: " + QString::number(joint.jointParam[3].actual)+" rad");
    ui->lbl_J5->setText("Wrist2: " + QString::number(joint.jointParam[4].actual)+" rad");
    ui->lbl_J6->setText("Wrist3: " + QString::number(joint.jointParam[5].actual)+" rad");
}
////////////////////////////////////////////////
//////// END EXTERNAL PUBLIC SLOTS /////////////
////////////////////////////////////////////////
///