#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <stdio.h>

char position1[1000];
char position2[1000];
char position3[1000];
char trama[1000];
char ini[1000];
char serv[1000];

int check;
/**
 * @brief MainWindow::MainWindow Única clase de la aplicación. Dibuja la gui, se encarga de la selección de botones, configura todas las signals y slots e inicializa los temporizadores
 * @param parent
 */
MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);

    _timer = new QTimer(this);
    connect(_timer,SIGNAL(timeout()),this, SLOT(update()));
    _timer->start(50);

    _timer2 = new QTimer(this);
    connect(_timer2,SIGNAL(timeout()),this, SLOT(updates()));
    _timer2->start(25);

    _timer3 = new QTimer(this);
    connect(_timer3,SIGNAL(timeout()),this, SLOT(automode()));

    _port = new QSerialPort();
    _port->setBaudRate(QSerialPort::Baud115200);
    _port->setDataBits(QSerialPort::Data8);
    _port->setParity(QSerialPort::NoParity);
    _port->setStopBits(QSerialPort::OneStop);
    _port->setFlowControl(QSerialPort::NoFlowControl);
    connect(_port,SIGNAL(readyRead()),this,SLOT(LeerPuerto()));
}
/**
 * @brief MainWindow::~MainWindow
 */
MainWindow::~MainWindow()
{
    delete ui;
}
/**
 * @brief MainWindow::update método encargado de la composición y envío de la trama para control de los actuadores en arduino y actualizar la trama en la GUI
 */
void MainWindow::update()
{
    ui->Inibyte->setText(QString::number(_initbyte));//convierte int a Qstring

    if (counter < 3){
        counter++;
    }
    else {
        counter = 1;
    }
    ui->Servo->setText(QString::number(counter));

    ui->Pos->setText(QString::number(_pos));
    if (_pos < 0){ui->Sign->setText("1");}
    else {ui->Sign->setText("0");}
    ui->Pos_2->setText(QString::number(_pos2));
    if (_pos2 < 0){ui->Sign_2->setText("1");}
    else {ui->Sign_2->setText("0");}
    ui->Pos_3->setText(QString::number(_pos3));
    if (_pos3 < 0){ui->Sign_3->setText("1");}
    else {ui->Sign_3->setText("0");}

    if (_transmit == 1){
        sprintf(position1,"%d",_pos);
        sprintf(position2,"%d",_pos2);
        sprintf(position3,"%d",_pos3);
        sprintf(ini,"%d",_initbyte);
        sprintf(serv,"%d",counter);
        strcpy(trama,ini);
        strcat(trama, " ");
        strcat(trama, serv);
        strcat(trama," ");
        switch (counter) {
        case 1:
            if (_pos >= 0){
                strcat(trama,"0 ");
            }
            else if (_pos < 0){
                strcat(trama,"1 ");
                _pos = -_pos;
            }
            if (_pos >=0 && _pos <10){
                strcat(trama,"00");
            }
            else if (_pos >= 10 && _pos<100){
                strcat(trama,"0");
            }
            sprintf(position1,"%d",_pos);
            strcat(trama, position1);
            break;
        case 2:
            if (_pos2 >= 0){
                strcat(trama,"0 ");
            }
            else if (_pos2 < 0){
                strcat(trama,"1 ");
                _pos2 = -_pos2;
            }
            if (_pos2 >=0 && _pos2 <10){
                strcat(trama,"00");
            }
            else if (_pos2 >= 10 && _pos2<100){
                strcat(trama,"0");
            }
            sprintf(position2,"%d",_pos2);
            strcat(trama, position2);
            break;
        case 3:
            if (_pos3 >= 0){
                strcat(trama,"0 ");
            }
            else if (_pos3 < 0){
                strcat(trama,"1 ");
                _pos3 = -_pos3;
            }
            if (_pos3 >=0 && _pos3 <10){
                strcat(trama,"00");
            }
            else if (_pos3 >= 10 && _pos3<100){
                strcat(trama,"0");
            }
            sprintf(position3,"%d",_pos3);
            strcat(trama, position3);
            break;
        default:
            break;
        }
        strcat(trama,"\n\r");
        _port->write(trama);
    }
}
/**
 * @brief MainWindow::on_horizontalSlider_valueChanged método que se encarga de actualizar un display del cual se toman datos en otros métodos y funciones
 * @param value valor del slider y valor a mostrar por en el display
 */
void MainWindow::on_horizontalSlider_valueChanged(int value)
{
    ui->lcdNumber_8->display(value);
}
/**
 * @brief MainWindow::on_InitStop_clicked método que se encarga de dar la orden de apagar o encender el electroimán
 */
void MainWindow::on_InitStop_clicked()
{
    ui->InitStop->setText("Stop");
    if (_initbyte == 0){
        _initbyte = 1;
        ui->InitStop->setText("Stop");
    }
    else {
        _initbyte = 0;
        ui->InitStop->setText("Init");
    }
}
/**
 * @brief MainWindow::on_transmit_clicked este método se encarga de intentar abrir el puerto y avisa en caso de exito o de fracaso
 */
void MainWindow::on_transmit_clicked()
{
    _port->setPortName("/dev/pts/11");
    printf("Abriendo el puerto ... ");
    if (_port->open(QIODevice::ReadWrite)) {
    printf("abierto!!!\n");
    _transmit = 1;
    }
    else {
    printf("vaya, esto CASCA\n");
    }
}
/**
 * @brief MainWindow::on_transmitStop_clicked este metodo cierra el puerto
 */
void MainWindow::on_transmitStop_clicked()
{
    _port->close();
    _transmit = 0;
}
/**
 * @brief MainWindow::on_verticalSlider_valueChanged método que se encarga de actualizar un display del cual se toman datos en otros métodos y funciones
 * @param value valor del slider y valor a mostrar por en el display
 */
void MainWindow::on_verticalSlider_valueChanged(int value)
{
    ui->lcdNumber_9->display(value);
}
/**
 * @brief MainWindow::on_verticalSlider_2_valueChanged método que se encarga de actualizar un display del cual se toman datos en otros métodos y funciones
 * @param value valor del slider y valor a mostrar por en el display
 */
void MainWindow::on_verticalSlider_2_valueChanged(int value)
{
    ui->lcdNumber_10->display(value);
}
/**
 * @brief MainWindow::delta_calcForward función que calcula la cinemática inversa
 * @param theta1 Ángulo del servo 1
 * @param theta2 Ángulo del servo 2
 * @param theta3 Ángulo del servo 3
 * @return este valor dice si es una posición posible o no
 */
float MainWindow::delta_calcForward(float theta1, float theta2, float theta3)
{
    float t = (f-e)*tan30/2;
    float dtr = pi/(float)180.0;

    float result[4];

    theta1 *= dtr;
    theta2 *= dtr;
    theta3 *= dtr;

    float y1 = -(t + rf*cos(theta1));
    float z1 = -rf*sin(theta1);

    float y2 = (t + rf*cos(theta2))*sin30;
    float x2 = y2*tan60;
    float z2 = -rf*sin(theta2);

    float y3 = (t + rf*cos(theta3))*sin30;
    float x3 = -y3*tan60;
    float z3 = -rf*sin(theta3);

    float dnm = (y2-y1)*x3-(y3-y1)*x2;

    float w1 = y1*y1 + z1*z1;
    float w2 = x2*x2 + y2*y2 + z2*z2;
    float w3 = x3*x3 + y3*y3 + z3*z3;

    // x = (a1*z + b1)/dnm
    float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
    float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;

    // y = (a2*z + b2)/dnm;
    float a2 = -(z2-z1)*x3+(z3-z1)*x2;
    float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;

    // a*z^2 + b*z + c = 0
    float a = a1*a1 + a2*a2 + dnm*dnm;
    float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
    float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re);

    // discriminant
    float d = b*b - (float)4.0*a*c;
    if (d < 0) {
        return -1;
    }// non-existing point

    posZ0 = -(float)0.5*(b+sqrt(d))/a;
    posX0 = (a1*posZ0 + b1)/dnm;
    posY0 = (a2*posZ0 + b2)/dnm;

    return 0;
}
/**
 * @brief MainWindow::delta_calcAngleYZ Función que calcula la cinemñatica inversa (solo uno de los ángulos)
 * @param x0 posición X del elector final
 * @param y0 posición Y del efector final
 * @param z0 posición Z del efector final
 * @return ángulo que ha de tomar el servo
 */
float MainWindow::delta_calcAngleYZ(float x0, float y0, float z0)
{
    float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30
    y0 -= 0.5 * 0.57735    * e;    // shift center to edge
    // z = a + b*y
    float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0);
    float b = (y1-y0)/z0;
    // discriminant
    float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf);
    if (d < 0) return -1; // non-existing point
    float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point
    float zj = a + b*yj;
    float theta = 180.0*atan(-zj/(y1 - yj))/pi + ((yj>y1)?180.0:0.0);
    return theta;
}
/**
 * @brief MainWindow::updates método que llama las funciones de cálculo de cinemática y actualiza los valores de unos displays con los resultados
 */
void MainWindow::updates()
{
    ui->lcdNumber->display(ui->lcdNumber_11->value());
    ui->lcdNumber_2->display(ui->lcdNumber_12->value());
    ui->lcdNumber_3->display(ui->lcdNumber_13->value());

    check = delta_calcForward(ui->lcdNumber->value(), ui->lcdNumber_2->value(), ui->lcdNumber_3->value());

    ui->lcdNumber_4->display(posX0);
    ui->lcdNumber_5->display(posY0);
    ui->lcdNumber_6->display(posZ0);
    ui->lcdNumber_7->display(check);

    theta1 = delta_calcAngleYZ(ui->lcdNumber_8->value(),
                               ui->lcdNumber_9->value(),
                               ui->lcdNumber_10->value());
    theta2 = delta_calcAngleYZ(ui->lcdNumber_8->value()*cos120 + ui->lcdNumber_9->value()*sin120,
                               ui->lcdNumber_9->value()*cos120-ui->lcdNumber_8->value()*sin120,
                               ui->lcdNumber_10->value());  // rotate coords to +120 deg
    theta3 = delta_calcAngleYZ(ui->lcdNumber_8->value()*cos120 - ui->lcdNumber_9->value()*sin120,
                               ui->lcdNumber_9->value()*cos120+ui->lcdNumber_8->value()*sin120,
                               ui->lcdNumber_10->value());
    ui->lcdNumber_11->display(theta1);
    ui->lcdNumber_12->display(theta2);
    ui->lcdNumber_13->display(theta3);

    _pos = ui->lcdNumber_11->value();
    _pos2 = ui->lcdNumber_12->value();
    _pos3 = ui->lcdNumber_13->value();
}
/**
 * @brief MainWindow::LeerPuerto lee la trama que viene del arduino con los datos de la cámara y en caso de que la trama recivida sea correcta y se quiera que se actualicen los datos actualiza uns displays
 */
void MainWindow::LeerPuerto()
{
        _port->read((char*)_buffer,BUFFMAX);
        sscanf(_buffer,"%*s %d %d %d %d %d",&_inicio, &_signature, &_objx, &_objy, &_final);

      if (_inicio == 500 && _final == 300 && _ManualCam == true){
          ui->lcdNumber_15->display(_signature);
          ui->lcdNumber_16->display(_objx);
          ui->lcdNumber_17->display(_objy);
      }
}
/**
 * @brief MainWindow::automode método que sirve de demostración
 */
void MainWindow::automode()
{
    if (_estado == 1){//arriba en inicio
        ui->horizontalSlider->setValue(56);
        ui->verticalSlider->setValue(-80);
        ui->verticalSlider_2->setValue(-120);
        _timer3->stop();
        _timer3->start(500);
        _estado = 2;//2
    }
    else if (_estado == 2){//abajo en inicio
        ui->verticalSlider_2->setValue(-168);
        _initbyte = 1;
        _estado = 3;
    }
    else if (_estado == 3){
        ui->verticalSlider_2->setValue(-120);
        _estado = 4;
    }
    else if (_estado == 4){
        ui->horizontalSlider->setValue(0);
        ui->verticalSlider->setValue(0);
        ui->lcdNumber_15->display(0);
        if (_color == 1){
            _estado = 5;//3
            _color = 0;
        }
        else if (_color == 2){
            _estado = 8;
            _color = 0;
        }
    }
    else if (_estado == 5){//arriba rojo
        ui->horizontalSlider->setValue(69);
        ui->verticalSlider->setValue(80);
        ui->verticalSlider_2->setValue(-120);
        _estado = 6;//4
    }
    else if (_estado == 6){//abajo rojo iman encendido
        ui->verticalSlider_2->setValue(-168);
        _estado = 7;
    }
    else if (_estado == 7){//abajo rojo imán apagado
        _initbyte = 0;
        ui->lcdNumber_15->display(0);
        _timer3->stop();
        _timer3->start(100);
        _estado = 0;
    }
    else if (_estado == 8){//arriba azul
        ui->horizontalSlider->setValue(-56);
        ui->verticalSlider->setValue(80);
        ui->verticalSlider_2->setValue(-120);
        _estado = 9;
    }
    else if (_estado == 9){//abajo azul iman encendido
        ui->verticalSlider_2->setValue(-168);
        _estado = 10;
    }
    else if (_estado == 10){// abajo azul imán apagado
        _initbyte = 0;
         ui->lcdNumber_15->display(0);
         _timer3->stop();
         _timer3->start(100);
        _estado = 0;
    }
    else if (_estado == 0){
        ui->horizontalSlider->setValue(0);
        ui->verticalSlider->setValue(0);
        ui->verticalSlider_2->setValue(-120);
        _color = ui->lcdNumber_15->value();
        _posX = ui->lcdNumber_16->value();
        _posY = ui-> lcdNumber_17->value();
        if (_color == 1 && _posX < 60 && _posX > 40 && _posY < 55 && _posY > 35 ||
            _color == 2 && _posX < 60 && _posX > 40 && _posY < 55 && _posY > 45 ){
            _estado = 1;
            _timer3->stop();
            _timer3->start(100);
        }
    }
}
/**
 * @brief MainWindow::on_InitStop_2_clicked método que dice si se quiere que los datos de la cámara sean actualizados o no
 */
void MainWindow::on_InitStop_2_clicked()
{
    if (_ManualCam == false){
        _ManualCam = true;
        ui->InitStop_2->setText("Manual");
    }
    else {
        _ManualCam = false;
        ui->InitStop_2->setText("Camera");
    }
}
/**
 * @brief MainWindow::on_pushButton_clicked método que activa o desactiva la demostración
 */
void MainWindow::on_pushButton_clicked()
{
    if (_auto == 0){
        ui->pushButton->setText("Stop Auto");
        _auto = 1;
        _timer3->start(100);
        _estado = 0;
    }dsafsf
    else {
        ui->pushButton->setText("Start Auto");
        _auto = 0;
        _timer3->stop();
        _estado = 20;
    }
}
