#include "UR3CalibWidget.h"
#include "ui_UR3CalibWidget.h"

const QVector<QString> labelsStringsJoints = {"Base[°]:", "Shoulder[°]:", "Elbow[°]:", "Wrist 1[°]:", "Wrist 2[°]:", "Wrist 3[°]:"};
const QVector<QString> labelsStringsCartesian = {"X[mm]:", "Y[mm]:", "Z[mm]:", "RX[rad]:", "RY[rad]:", "RZ[rad]:"};

double rad2deg(double rad)
{
    return rad * 180.0 / 3.14159265359;
}

double deg2rad(double deg)
{
    return deg * 3.14159265359 / 180.0;
}

UR3CalibWidget::UR3CalibWidget(UR3Workspace *UR3W, QWidget *parent) : QWidget(parent), ui(new Ui::UR3CalibWidget), UR3Workspace_(UR3W)
{
    ui->setupUi(this);

    for(int i=0; i<6; i++)
    {
        QLabel* lbl = new QLabel(labelsStringsJoints[i], this);
        labelsPointers_.push_back(lbl);
        ui->layout_labels->insertWidget(i, lbl);

        QLineEdit* save_line = new QLineEdit(this);
        savedLinesPointers_.push_back(save_line);
        ui->layout_saved->insertWidget(i, save_line);

        QLineEdit* current_line = new QLineEdit(this);
        current_line->setEnabled(false);
        currentLinesPointers_.push_back(current_line);
        ui->layout_current->insertWidget(i, current_line);
    }

    connect(UR3Workspace_, SIGNAL(newPose(QVector<double>,char)), this, SLOT(newPoseFromRobot(QVector<double>,char)));    
}

UR3CalibWidget::~UR3CalibWidget()
{
    delete ui;
}

void UR3CalibWidget::startCalibration()
{
    UR3Workspace_->setCalibrationTCP();

    calibrationState_ = HOME;

    enableBinArrayMode_ = UR3Workspace_->getEnableBinArrayMode();
    if(enableBinArrayMode_ == false)
    {
        binDescriptors_ = UR3Workspace_->getBinDescriptors();
    }else
    {
        arrayStartPosition_ = UR3Workspace_->getArrayStartPosition();
        arrayEndPosition_ = UR3Workspace_->getArrayEndPosition();
    }

    homePosition_ = UR3Workspace_->getHomePosition();
    boardPosition_ = UR3Workspace_->getBoardPosition();
    overBinPosition_ = UR3Workspace_->getOverBinPosition();
    calibrationPoints_ = UR3Workspace_->getCalibrationPoints();

    calibPointsIterator_ = calibrationPoints_.begin();
    binIterator_ = binDescriptors_.begin();

    on_btn_next_clicked();
}

void UR3CalibWidget::setJointStrings()
{
    for(int i=0; i<6; i++)
    {
        labelsPointers_[i]->setText(labelsStringsJoints[i]);
    }

    currentDataType_ = 'j';
}

void UR3CalibWidget::setCartesianStrings()
{
    for(int i=0; i<6; i++)
    {
        labelsPointers_[i]->setText(labelsStringsCartesian[i]);
    }

    currentDataType_ = 'p';
}

void UR3CalibWidget::setEditsValues(QVector<double> data, char type)
{
    if(type == 'j')
    {
        for(int i=0; i<6; i++)
        {
            savedLinesPointers_[i]->setText(QString::number(rad2deg(data[i])));
        }
    }

    if(type == 'p')
    {
        for(int i=0; i<3; i++)
        {
            savedLinesPointers_[i]->setText(QString::number(1000.0 * data[i]));
        }
        for(int i=3; i<6; i++)
        {
            savedLinesPointers_[i]->setText(QString::number(data[i]));
        }
    }
}

QVector<double> UR3CalibWidget::getEditValues(char type)
{
    QVector<double> data;

    if(type == 'j')
    {
        for(int i=0; i<6; i++)
        {
            data.push_back(deg2rad(savedLinesPointers_[i]->text().toDouble()));
        }
    }

    if(type == 'p')
    {
        for(int i=0; i<3; i++)
        {
            data.push_back(savedLinesPointers_[i]->text().toDouble()/1000.0);
        }
        for(int i=3; i<6; i++)
        {
            data.push_back(savedLinesPointers_[i]->text().toDouble());
        }
    }

    return data;
}

void UR3CalibWidget::closeEvent(QCloseEvent *event)
{
    UR3Workspace_->setWorkingTCP();
    //UR3Workspace_->setFreedriveMode(false);
    QWidget::closeEvent(event);
}

void UR3CalibWidget::newPoseFromRobot(QVector<double> data, char type)
{
    if(type == currentDataType_ && data.size() == 6)
    {
        if(currentDataType_ == 'j')
        {
            for(int i=0; i<6; i++)
            {
                currentLinesPointers_[i]->setText(QString::number(rad2deg(data[i])));
            }
        }

        if(currentDataType_ == 'p'){
            for(int i=0; i<3; i++)
            {
                currentLinesPointers_[i]->setText(QString::number(1000.0 * data[i]));
            }
            for(int i=3; i<6; i++)
            {
                currentLinesPointers_[i]->setText(QString::number(data[i]));
            }
        }
    }
}

void UR3CalibWidget::show()
{
    startCalibration();
    QWidget::show();
}

void UR3CalibWidget::on_btn_next_clicked()
{
    switch(calibrationState_)
    {
        case HOME:
            setJointStrings();
            setEditsValues(homePosition_, 'j');
            ui->lbl_current_point->setText("Set HOME position");
            calibrationState_ = BOARD;
            break;
        case BOARD:
            homePosition_ = getEditValues('j');

            setEditsValues(boardPosition_, 'j');
            ui->lbl_current_point->setText("Set position OVER BOARD");
            calibrationState_ = CALIB_POINTS;
            break;
        case CALIB_POINTS:
            if(calibPointsIterator_ == calibrationPoints_.begin())
            {
                boardPosition_ = getEditValues('j');
            }else
            {
                (calibPointsIterator_-1).value() = getEditValues('p');
            }

            setCartesianStrings();
            setEditsValues(calibPointsIterator_.value(), 'p');
            ui->lbl_current_point->setText("Touch point P" + QString::number(calibPointsIterator_.key()) + " on the board");

            calibPointsIterator_++;

            if(calibPointsIterator_ == calibrationPoints_.end())
            {
                calibrationState_ = OVER_BINS;
            }
            break;
        case OVER_BINS:
            (calibPointsIterator_-1).value() = getEditValues('p');

            setJointStrings();
            setEditsValues(overBinPosition_, 'j');
            ui->lbl_current_point->setText("Set position OVER BINS");

            if(enableBinArrayMode_ == true)
            {
                UR3Workspace_->setCalibrationTCP();
                calibrationState_ = BIN_ARRAY_START;
            }else
            {
                UR3Workspace_->setWorkingTCP();
                calibrationState_ = BINS;
            }
            break;
        case BINS:
            if(binIterator_ == binDescriptors_.begin())
            {
                overBinPosition_ = getEditValues('j');
            }else
            {
                (binIterator_-1).value().position = getEditValues('p');
            }

            setCartesianStrings();
            setEditsValues(binIterator_.value().position, 'p');
            ui->lbl_current_point->setText("Put robot over bin: " + binIterator_.value().name);

            binIterator_++;

            if(binIterator_ == binDescriptors_.end())
            {
                calibrationState_ = FINISH;
            }
            break;
        case BIN_ARRAY_START:
            overBinPosition_ = getEditValues('j');

            setCartesianStrings();
            setEditsValues(arrayStartPosition_, 'p');
            ui->lbl_current_point->setText("Touch START position of bin array");

            calibrationState_ = BIN_ARRAY_END;

            break;
        case BIN_ARRAY_END:
            arrayStartPosition_ = getEditValues('p');

            setEditsValues(arrayEndPosition_, 'p');
            ui->lbl_current_point->setText("Touch END position of bin array");

            calibrationState_ = FINISH;

            break;
        case FINISH:
            if(enableBinArrayMode_ == true)
            {
                arrayEndPosition_ = getEditValues('p');
            }else
            {
                (binIterator_-1).value().position = getEditValues('p');
            }

            UR3Workspace_->setHomePosition(homePosition_);
            UR3Workspace_->setBoardPosition(boardPosition_);
            UR3Workspace_->setOverBinPosition(overBinPosition_);

            if(enableBinArrayMode_ == false)
            {
                for(auto it = binDescriptors_.begin(); it != binDescriptors_.end(); it++)
                {
                    UR3Workspace_->setBinPosition(it.key(), it.value().position);
                }
            }else
            {
                UR3Workspace_->setArrayStartPosition(arrayStartPosition_);
                UR3Workspace_->setArrayEndPosition(arrayEndPosition_);
                UR3Workspace_->recalculateBinArray();
            }

            for(auto it = calibrationPoints_.begin(); it != calibrationPoints_.end(); it++)
            {
                UR3Workspace_->setCalibrationPoint(it.key(), it.value());
            }
            UR3Workspace_->recalculateCalibration();

            ui->lbl_current_point->setText("FINISHED");            

            emit calibrationFinished(true);
            this->close();

            break;
    }
}


void UR3CalibWidget::on_btn_cancel_clicked()
{
    emit calibrationFinished(false);
    this->close();
}

void UR3CalibWidget::on_btn_transfer_clicked()
{
    for(int i=0; i<6; i++)
    {
        savedLinesPointers_[i]->setText(currentLinesPointers_[i]->text());
    }
}
