
#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <QTcpSocket>
#include <QDateTime>
#include <anchor.h>

enum arrayPossition
{posCmd, posAnchor, posTag, posRssi}
possition;

enum Number
{zero, one, two}
tagNum;


MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
    QList<QHostAddress> vsechny_adresy = QNetworkInterface::allAddresses();
    foreach(QHostAddress a, vsechny_adresy)
    {
         if(a.protocol()==QAbstractSocket::IPv4Protocol)
            adresy.append(a);
    }
    connect(&server,SIGNAL(newConnection()),this,SLOT(new_connection()));

    if(!server.listen(QHostAddress::AnyIPv4,1234))  //1234...PORT NUMBER
        {
            qDebug()<<"Server nelze nastartovat";
        }
        else
        {
            qDebug()<<"Posloucha";
        }

    connect(ui->setAPosition,SIGNAL(clicked(bool)),this,SLOT(on_SetAPossitionClicked()));
    connect(ui->showAPosition,SIGNAL(clicked(bool)),this,SLOT(on_ShowAPossitionClicked()));


    connect(ui->rbT0,SIGNAL(clicked(bool)),this,SLOT(on_rButtonT0Clicked()));
    connect(ui->rbT1,SIGNAL(clicked(bool)),this,SLOT(on_rButtonT1Clicked()));
    connect(ui->rbT2,SIGNAL(clicked(bool)),this,SLOT(on_rButtonT2Clicked()));

    plotInit();
    on_ShowAPossitionClicked();


}

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

/*
 * new connection to server
 * */
void MainWindow::new_connection()
{
    qDebug()<<"pokus o nove pripojeni";
    QTcpSocket* socket = server.nextPendingConnection();
    connect(socket,SIGNAL(readyRead()),this,SLOT(new_data()));
    //socket->write("&&&");

}

/*
 * recieved data from client
 * */
void MainWindow::new_data()
{
    QTcpSocket* psender = (QTcpSocket*)sender();
    QByteArray recieved = psender->readAll();
    // //////
    QString sRecieved(recieved);

    //ui->plainTextEdit->appendPlainText(sRecieved);
    // //////
    if(recieved[posCmd] == 'G')
         getData(recieved,psender);

    if(recieved[posCmd] == 'S')
         setData(recieved);
}

/*
 * set recieved RSSI and DateTime to apropriete Tag and Anchor
 * */
void MainWindow::setData(QByteArray recieved)
{
    int anchorNum = (int) recieved[posAnchor];
    int tagNum = (int) recieved[posTag];
    int rssi = (int) recieved[posRssi];

    QDateTime date = QDateTime::currentDateTime();
    showRecievedInConsole(recieved, date);
    switch (tagNum)
    {
        case 0:
        {
            tag0[anchorNum].setRssi(rssi);
            tag0[anchorNum].setDate(date);
            showChangedData(tag0[anchorNum], recieved);
            tag0Trilateration[2] = tag0Trilateration[1];
            tag0Trilateration[1] = tag0Trilateration[0];
            tag0Trilateration[0] = anchorNum;
            if(ui->rbT0->isChecked())   //show in the plot if Radio Button 0 Checked
                on_rButtonT0Clicked();
            break;
        }
        case 1:
        {
            tag1[anchorNum].setRssi(rssi);
            tag1[anchorNum].setDate(date);
            showChangedData(tag1[anchorNum], recieved);
            tag1Trilateration[2] = tag1Trilateration[1];
            tag1Trilateration[1] = tag1Trilateration[0];
            tag1Trilateration[0] = anchorNum;
            if(ui->rbT1->isChecked())
                on_rButtonT1Clicked();
            break;
        }
        case 2:
        {
            tag2[anchorNum].setRssi(rssi);
            tag2[anchorNum].setDate(date);
            showChangedData(tag2[anchorNum], recieved);
            tag2Trilateration[2] = tag2Trilateration[1];
            tag2Trilateration[1] = tag2Trilateration[0];
            tag2Trilateration[0] = anchorNum;
            if(ui->rbT2->isChecked())
                on_rButtonT2Clicked();
        }
    }
}

/*
 * reply to client with apropriate RSSI and DateTime
 * */
void MainWindow::getData(QByteArray recieved, QTcpSocket *client)
{
    int anchorNum = (int) recieved[posAnchor]-'0';
    int tagNum = (int) recieved[posTag]-'0';
    int rssi;
    int trilat;
    QDateTime date;

    switch (tagNum)
    {
        case 0:
        {
            rssi = tag0[anchorNum].getRssi();
            date = tag0[anchorNum].getDate();
            trilat = (tag0Trilateration[0]+1)*100 + (tag0Trilateration[1]+1)*10 + (tag0Trilateration[2]+1);
            break;
        }
        case 1:
        {
            rssi = tag1[anchorNum].getRssi();
            date = tag1[anchorNum].getDate();
            trilat = (tag1Trilateration[0]+1)*100 + (tag1Trilateration[1]+1)*10 + (tag1Trilateration[2]+1);
            break;
        }
        default:
        {
            rssi = tag2[anchorNum].getRssi();
            date = tag2[anchorNum].getDate();
            trilat = (tag2Trilateration[0]+1)*100 + (tag2Trilateration[1]+1)*10 + (tag2Trilateration[2]+1);
        }
    }

    QString sAnswer = QString::number(anchorNum) + QString::number(tagNum) + date.toString("yyyy-MM-dd-HH:mm:ss") + QString::number(rssi) + QString::number(trilat);
    client->write(sAnswer.toUtf8());

    //ui->pteConsole->appendPlainText(sAnswer);// //////test version to control the answer in console

}

/*
 * Clear and Set QPlainTextEdit showing RSSI and DateTime of apropriate Tag and Anchor
 * */
void MainWindow::pteClearSet(QPlainTextEdit *pteDate, QPlainTextEdit *pteRssi, QString sDate, QString sRssi)
{
    pteDate->clear();
    pteRssi->clear();
    pteDate->appendPlainText(sDate);
    sRssi.prepend("-");
    pteRssi->appendPlainText(sRssi);
}

/*
 * Initialization of QCustomPlot
 * */
void MainWindow::plotInit()
{
    // set some basic customPlot config:
    ui->customPlot->setBackground(QPixmap("./abc.jpg"));
    //ui->customPlot->setInteractions(QCP::iSelectPlottables);

    ui->customPlot->axisRect()->setupFullAxesBox();
    ui->customPlot->xAxis->setRange(-1,10.25);
    ui->customPlot->xAxis->setVisible(false);
    ui->customPlot->xAxis2->setVisible(false);
    ui->customPlot->xAxis->grid()->setSubGridVisible(false);
    ui->customPlot->yAxis->setRange(-1,12);
    ui->customPlot->yAxis->setVisible(false);
    ui->customPlot->yAxis2->setVisible(false);
    ui->customPlot->yAxis->grid()->setSubGridVisible(false);

//pudorys
    drawLine(0,0,9.25,0,0);
    drawLine(9.25,0,9.25,11,1);
    drawLine(3.75,11,9.25,11,2);
    drawLine(3.75,9,3.75,11,3);
    drawLine(0,9,3.75,9,4);
    drawLine(0,0,0,9,5);
    drawLine(0,3.75,4.25,3.75,6);
    drawLine(5.5,3.75,9.25,3.75,7);
    drawLine(5.5,7.75,9.25,7.75,8); //nosny a obvodovy
    drawLine(5.5,0,5.5,2.25,9);
    drawLine(5.5,3.05,5.5,3.75,10);
    drawLine(5.5,3.75,5.5,4.75,11);
    drawLine(5.5,5.65,5.5,7.9,12);
    drawLine(5.5,8.8,5.5,9,13);
    drawLine(5.2,9,9.25,9,14);
    drawLine(3.75,9,4.3,9,15);
    drawLine(0,6.2,0.5,6.2,16);
    drawLine(1.4,6.2,4.25,6.2,17);
    drawLine(1.8,6.2,1.8,9,18);
    drawLine(4.25,3.75,4.25,4.25,19);
    drawLine(4.25,5.15,4.25,7,20);
    drawLine(4.25,7.9,4.25,9,21);

}

/*
 * [x0,y0] start point, [x1,y1] end point
 * */
void MainWindow::drawLine(double x0, double y0, double x1, double y1, int graphNumber)
{
    ui->customPlot->addGraph();
    QVector<double> x(100), y(100);
    if(y0 == y1) //horizontal
    {
        for (int i=0; i<100; ++i)
        {
        x[i] = i/(100/(x1-x0)) + x0;
        y[i] = y0;
        }
    }
    if(x0 == x1)    //vertical
    {
        for (int i=0; i<100; ++i)
        {
        x[i] = x0;
        y[i] = i/(100/(y1-y0)) + y0;
        }
    }
    QPen pen;
    pen.setColor(Qt::black);
    if(graphNumber < 9)
        pen.setWidth(5);
    else
        pen.setWidth(2);

    ui->customPlot->graph(graphNumber)->setPen(pen);
    ui->customPlot->graph(graphNumber)->setData(x, y);
}

/*
 * displays the circle of apropriate tag and anchor
 * */
void MainWindow::showCircle(Anchor &rtag, QCPCurve* Circle)
{
    // generate the curve data points:
    const int pointCount = 500;
    QVector<QCPCurveData> dataCircle(pointCount);
    for (int i=0; i<pointCount; ++i)
    {
      double phi = i/(double)(pointCount-1)*2*M_PI;
      dataCircle[i] = QCPCurveData(i, rtag.getX() + rtag.getRadius()*qCos(phi), rtag.getY() + rtag.getRadius()*qSin(phi));
    }
    // pass the data to the curves; we know t (i in loop above) is ascending, so set alreadySorted=true (saves an extra internal sort):
    Circle->data()->set(dataCircle, true);
    ui->customPlot->replot();
}

/*
 * displays the circle with different color choosen by recieved time
 * */
void MainWindow::showCircle(Anchor &rtag, QCPCurve* Circle, int sequence)
{
    // generate the curve data points:
    const int pointCount = 500;
    QVector<QCPCurveData> dataCircle(pointCount);
    for (int i=0; i<pointCount; ++i)
    {
      double phi = i/(double)(pointCount-1)*2*M_PI;
      dataCircle[i] = QCPCurveData(i, rtag.getX() + rtag.getRadius()*qCos(phi), rtag.getY() + rtag.getRadius()*qSin(phi));
    }
    // pass the data to the curves; we know t (i in loop above) is ascending, so set alreadySorted=true (saves an extra internal sort):
    Circle->data()->set(dataCircle, true);
    if(sequence == 1)
    {
        Circle->setPen(QPen(QColor(0, 128, 255, 20)));
        Circle->setBrush(QBrush(QColor(0, 128, 255, 120)));
    }
    if(sequence == 2)
    {
        Circle->setPen(QPen(QColor(0, 128, 255, 20)));
        Circle->setBrush(QBrush(QColor(0, 128, 255, 80)));
    }
    if(sequence == 3)
    {
        Circle->setPen(QPen(QColor(0, 128, 255, 20)));
        Circle->setBrush(QBrush(QColor(0, 128, 255, 40)));
    }
    ui->customPlot->replot();
}

/*
 * displays the circle found by trilateration
 * */
void MainWindow::showCircle(QVector3D &rPoint, QCPCurve* Circle)
{
    // generate the curve data points:
    const int pointCount = 500;
    QVector<QCPCurveData> dataCircle(pointCount);
    for (int i=0; i<pointCount; ++i)
    {
      double phi = i/(double)(pointCount-1)*2*M_PI;
      dataCircle[i] = QCPCurveData(i, rPoint.x() + 0.25*qCos(phi), rPoint.y() + 0.25*qSin(phi));
    }
    // pass the data to the curves; we know t (i in loop above) is ascending, so set alreadySorted=true (saves an extra internal sort):
    Circle->data()->set(dataCircle, true);
    Circle->setPen(QPen(QColor(204, 0, 0, 20)));
    Circle->setBrush(QBrush(QColor(204, 0, 0, 220)));
    ui->customPlot->replot();
}

/*
 * SLOT when Radio Button T0 Clicked clear plot, create QCPCurves and pass them to showCircle
 * */
void MainWindow::on_rButtonT0Clicked()
{
    int plotabbles;
    plotabbles = ui->customPlot->QCustomPlot::plottableCount();
    while(plotabbles > 22)
        ui->customPlot->QCustomPlot::removePlottable(--plotabbles);

    ui->customPlot->replot();
    trilaterate(0, &tag0Trilateration[0]);

}

/*
 * SLOT when Radio Button T1 Clicked clear plot, create QCPCurves and pass them to showCircle
 * */
void MainWindow::on_rButtonT1Clicked()
{
    int plotabbles;
    plotabbles = ui->customPlot->QCustomPlot::plottableCount();
     while(plotabbles > 22)
         ui->customPlot->QCustomPlot::removePlottable(--plotabbles);

    ui->customPlot->replot();
    trilaterate(1, &tag1Trilateration[0]);
}

/*
 * SLOT when Radio Button T2 Clicked clear plot, create QCPCurves and pass them to showCircle
 * */
void MainWindow::on_rButtonT2Clicked()
{
    int plotabbles;
    plotabbles= ui->customPlot->QCustomPlot::plottableCount();
    while(plotabbles > 22)
        ui->customPlot->QCustomPlot::removePlottable(--plotabbles);

    ui->customPlot->replot();
    trilaterate(2, &tag2Trilateration[0]);

}

void MainWindow::trilaterate(int tagNumber, int *ptagTrilateration)
{
    QVector3D point1 = QVector3D((double) tag0[*ptagTrilateration].getX(), (double) tag0[*ptagTrilateration].getY(), (double) 0);
    QVector3D point2 = QVector3D((double) tag0[*(ptagTrilateration +1)].getX(), (double) tag0[*(ptagTrilateration +1)].getY(), (double) 0);
    QVector3D point3 = QVector3D((double) tag0[*(ptagTrilateration +2)].getX(), (double) tag0[*(ptagTrilateration +2)].getY(), (double) 0);

    double d1, d2, d3;

    if(tagNumber == 0)
    {
        d1 = (double)tag0[*ptagTrilateration].getRadius();
        d2 = (double)tag0[*(ptagTrilateration +1)].getRadius();
        d3 = (double)tag0[*(ptagTrilateration +2)].getRadius();
    }
    if(tagNumber == 1)
    {
        d1 = (double)tag1[*ptagTrilateration].getRadius();
        d2 = (double)tag1[*(ptagTrilateration +1)].getRadius();
        d3 = (double)tag1[*(ptagTrilateration +2)].getRadius();
    }
    if(tagNumber == 2)
    {
        d1 = (double)tag2[*ptagTrilateration].getRadius();
        d2 = (double)tag2[*(ptagTrilateration +1)].getRadius();
        d3 = (double)tag2[*(ptagTrilateration +2)].getRadius();
    }
    else
    {
        d1 = 0;
        d2 = 0;
        d3 = 0;
    }

    QVector3D point1m = point1 - point1;
    QVector3D point2m = point2 - point1;
    QVector3D point3m = point3 - point1;

    double thetaRadian = acos((double)point2m.x()/sqrt((double)point2m.x() + (double)point2m.y() +(double)point2m.z()));

    QVector3D point1r = point1m;
    QVector3D point2r = QVector3D((double)point2m.x()*cos(thetaRadian *-1)-(double)point2m.y()*sin(thetaRadian*-1),
                                  0,
                                  0);
    QVector3D point3r = QVector3D((double)point3m.x()*cos(thetaRadian *-1)-(double)point3m.y()*sin(thetaRadian*-1),
                                  (double)point3m.x()*sin(thetaRadian *-1)+(double)point3m.y()*cos(thetaRadian*-1),
                                  0);

    double x = (pow(d1,2)-pow(d2,2) + pow((double)point2r.x(),2))/(2*(double)point2r.x());
    double y = (pow(d1,2)-pow(d3,2) + pow((double)point3r.x(),2) + pow((double)point3r.y(),2))
                /(2*(double)point3r.y() - ((double)point3r.x()/(double)point3r.y())*x);
    QVector3D pointTri = QVector3D(x*cos(thetaRadian)-y*sin(thetaRadian),
                                   x*sin(thetaRadian)+y*cos(thetaRadian),
                                   0);
    pointTri += point1;

    QCPCurve *circle0 =  new QCPCurve(ui->customPlot->xAxis, ui->customPlot->yAxis);
    QCPCurve *circle1 =  new QCPCurve(ui->customPlot->xAxis, ui->customPlot->yAxis);
    QCPCurve *circle2 =  new QCPCurve(ui->customPlot->xAxis, ui->customPlot->yAxis);

    if(tagNumber == 0)
    {
        showCircle(tag0[*ptagTrilateration], circle0, 1);
        showCircle(tag0[*(ptagTrilateration +1)], circle1, 2);
        showCircle(tag0[*(ptagTrilateration +2)], circle2, 3);
    }
    if(tagNumber == 1)
    {
        showCircle(tag1[*ptagTrilateration], circle0, 1);
        showCircle(tag1[*(ptagTrilateration +1)], circle1, 2);
        showCircle(tag1[*(ptagTrilateration +2)], circle2, 3);
    }
    if(tagNumber == 2)
    {
       showCircle(tag2[*ptagTrilateration], circle0, 1);
       showCircle(tag2[*(ptagTrilateration +1)], circle1, 2);
       showCircle(tag2[*(ptagTrilateration +2)], circle2, 3);
    }


    QCPCurve *circle =  new QCPCurve(ui->customPlot->xAxis, ui->customPlot->yAxis);
    showCircle(pointTri, circle);
    qDebug() << "New Point : " << QString::number(pointTri.x()) << " " << QString::number(pointTri.y()) << " " << QString::number(pointTri.z());

}

/*
 * get double value from QPlainTextEdit QString value - setting Anchor possition
 * */
void MainWindow::pteGetDouble(QPlainTextEdit* pteXP, QPlainTextEdit* pteYP, int AnchorNumber)
{
    QString sPossition = pteXP->toPlainText();
    bool conversionOk;
    double dPossition = sPossition.toDouble(&conversionOk);
    if(conversionOk)    //if invalid character -> leave default value
    {
        tag0[AnchorNumber].setX(dPossition);
        tag1[AnchorNumber].setX(dPossition);
        tag2[AnchorNumber].setX(dPossition);
    }


    sPossition = pteYP->toPlainText();
    dPossition = sPossition.toDouble(&conversionOk);

    if(conversionOk)
    {
        tag0[AnchorNumber].setY(sPossition.toDouble());
        tag1[AnchorNumber].setY(sPossition.toDouble());
        tag2[AnchorNumber].setY(sPossition.toDouble());
    }

}

/*
 * SLOT for - Set Anchor Possition Button clicked
 * */
void MainWindow::on_SetAPossitionClicked()
{
    pteGetDouble(ui->pteA0X, ui->pteA0Y,0);
    pteGetDouble(ui->pteA1X, ui->pteA1Y,1);
    pteGetDouble(ui->pteA2X, ui->pteA2Y,2);
    pteGetDouble(ui->pteA3X, ui->pteA3Y,3);
    pteGetDouble(ui->pteA4X, ui->pteA4Y,4);
    pteGetDouble(ui->pteA5X, ui->pteA5Y,5);
}

/*
 * SLOT for - Show Anchor Possition Button clicked
 * */
void MainWindow::on_ShowAPossitionClicked()
{
    pteSetString(ui->pteA0X, ui->pteA0Y,0);
    pteSetString(ui->pteA1X, ui->pteA1Y,1);
    pteSetString(ui->pteA2X, ui->pteA2Y,2);
    pteSetString(ui->pteA3X, ui->pteA3Y,3);
    pteSetString(ui->pteA4X, ui->pteA4Y,4);
    pteSetString(ui->pteA5X, ui->pteA5Y,5);
    if(ui->rbT0->isChecked())
        on_rButtonT0Clicked();
    if(ui->rbT1->isChecked())
        on_rButtonT1Clicked();
    if(ui->rbT2->isChecked())
        on_rButtonT2Clicked();

}

/*
 * get double value of Anchor Possition and present it as QString in QPlainTextEdit showing Anchor Possition
 * */
void MainWindow::pteSetString(QPlainTextEdit* pteXP, QPlainTextEdit* pteYP, int AnchorNumber)
{
    double possition;
    possition = tag0[AnchorNumber].getX();
    QString sPossition = QString::number(possition,'f',1);
    pteXP->setPlainText(sPossition);
    possition = tag0[AnchorNumber].getY();
    sPossition = QString::number(possition,'f',1);
    pteYP->setPlainText(sPossition);

}

/*
 * show newly recieved data in apropriate QPlainTextEdit
 * */
void MainWindow::showChangedData(Anchor &rtag, QByteArray recieved)
{
    QString sDate = rtag.getDate().toString("dd.MM.yyyy  hh:mm:ss");
    //QString sRssi(rtag.getRssi());
    QString sRssi = QString::number(rtag.getRssi());
    char tagNumber = recieved[posTag];
    char anchorNumber = recieved[posAnchor];
    switch (tagNumber)
    {
        case 0:
        {
            switch(anchorNumber)
            {
            case 0:
            pteClearSet(ui->pteT0A0Date, ui->pteT0A0Rssi, sDate, sRssi);
            break;
            case 1:
            pteClearSet(ui->pteT0A1Date, ui->pteT0A1Rssi, sDate, sRssi);
            break;
            case 2:
            pteClearSet(ui->pteT0A2Date, ui->pteT0A2Rssi, sDate, sRssi);
            break;
            case 3:
            pteClearSet(ui->pteT0A3Date, ui->pteT0A3Rssi, sDate, sRssi);
            break;
            case 4:
            pteClearSet(ui->pteT0A4Date, ui->pteT0A4Rssi, sDate, sRssi);
            break;
            case 5:
            pteClearSet(ui->pteT0A5Date, ui->pteT0A5Rssi, sDate, sRssi);
            }
        break;
        }
        case 1:
        {
            switch(anchorNumber)
            {
            case 0:
            pteClearSet(ui->pteT1A0Date, ui->pteT1A0Rssi, sDate, sRssi);
            break;
            case 1:
            pteClearSet(ui->pteT1A1Date, ui->pteT1A1Rssi, sDate, sRssi);
            break;
            case 2:
            pteClearSet(ui->pteT1A2Date, ui->pteT1A2Rssi, sDate, sRssi);
            break;
            case 3:
            pteClearSet(ui->pteT1A3Date, ui->pteT1A3Rssi, sDate, sRssi);
            break;
            case 4:
            pteClearSet(ui->pteT1A4Date, ui->pteT1A4Rssi, sDate, sRssi);
            break;
            case 5:
            pteClearSet(ui->pteT1A5Date, ui->pteT1A5Rssi, sDate, sRssi);
            }
        break;
        }
        case 2:
        {
            switch(anchorNumber)
            {
            case 0:
            pteClearSet(ui->pteT2A0Date, ui->pteT2A0Rssi, sDate, sRssi);
            break;
            case 1:
            pteClearSet(ui->pteT2A1Date, ui->pteT2A1Rssi, sDate, sRssi);
            break;
            case 2:
            pteClearSet(ui->pteT2A2Date, ui->pteT2A2Rssi, sDate, sRssi);
            break;
            case 3:
            pteClearSet(ui->pteT2A3Date, ui->pteT2A3Rssi, sDate, sRssi);
            break;
            case 4:
            pteClearSet(ui->pteT2A4Date, ui->pteT2A4Rssi, sDate, sRssi);
            break;
            case 5:
            pteClearSet(ui->pteT2A5Date, ui->pteT2A5Rssi, sDate, sRssi);
            }
        }
    }
}

void MainWindow::showRecievedInConsole(QByteArray recieved, QDateTime currentTime)
{
    QString sRecieved = currentTime.toString("hh:mm:ss  dd.MM.yyyy") + "    Tag#" + QString::number(recieved[posTag])+
           "  Anchor#"+ QString::number(recieved[posAnchor])  +" RSSI: -" +QString::number(recieved[posRssi]);
    ui->pteConsole->appendPlainText(sRecieved);
}
