#include "mainwindow.h"
#include "./ui_mainwindow.h"
#include <QLibrary>
#include <QHostAddress>
#include <QFileDialog>
#include <QDateTime>

extern "C"{
#include "SIL_capi.h"
}

void MainWindow::scan_param()
{
    const rtwCAPI_ModelMappingStaticInfo *sm = (*getmap_func)();
    if (sm)
    {
        //unsigned int n = rtwCAPI_GetNumBlockParametersFromStaticMap(sm);
        unsigned int m = rtwCAPI_GetNumModelParametersFromStaticMap(sm);
        ui->tableWidget->setRowCount(512);
        QStringList hdr;
        hdr.push_back(tr("Parameter"));
        hdr.push_back(tr("Value"));
        ui->tableWidget->setHorizontalHeaderLabels(hdr);
        int cur = 0;

        rtwCAPI_ModelParameters const *prm = rtwCAPI_GetModelParametersFromStaticMap(sm);
        rtwCAPI_DataTypeMap const *dtm = rtwCAPI_GetDataTypeMapFromStaticMap(sm);
        rtwCAPI_DimensionMap const *dmm = rtwCAPI_GetDimensionMapFromStaticMap(sm);
        uint_T const *dam = rtwCAPI_GetDimensionArrayFromStaticMap(sm);
        rtwCAPI_ModelMappingInfo *MMI = &(rtmGetDataMapInfo(this->m).mmi);
        void **da = rtwCAPI_GetDataAddressMap(MMI);
        for (unsigned int i = 0; i < m; ++i)
        {
            const char *name = rtwCAPI_GetModelParameterName(prm, i);
            int idx = rtwCAPI_GetModelParameterAddrIdx(prm, i);
            uint16_t dti = rtwCAPI_GetModelParameterDataTypeIdx(prm, i);
            uint16_t dmi = rtwCAPI_GetModelParameterDimensionIdx(prm, i);
            uint8_t ss = rtwCAPI_GetDataTypeSLId(dtm, dti);
            uint16_t dai = rtwCAPI_GetDimArrayIndex(dmm, dmi);
            uint8_t nd = rtwCAPI_GetNumDims(dmm, dmi);
            uint16_t n = 1;
            for (int j = 0; j < nd; ++j)
            {
                n *= dam[dai + j];
            }
            for (int j = 0; j < n; ++j)
            {
                QString param_name = QString::fromLatin1(name);
                if (j>0)
                {
                    param_name = param_name+QString("(%1)").arg(j);
                }
                QString param_value;
                switch (ss)
                {
                case SS_DOUBLE:
                {
                    double *d = (double *)da[idx];
                    param_value = QString("%1").arg(d[j]);
                }
                break;
                case SS_SINGLE:
                {
                    float *s = (float *)da[idx];
                    param_value = QString("%1").arg(s[j]);
                }
                break;
                case SS_INT8:
                {
                    int8_T *c = (int8_T *)da[idx];
                    param_value = QString("%1").arg(c[j]);
                }
                break;
                case SS_INT16:
                {
                    int16_T *h = (int16_T *)da[idx];
                    param_value = QString("%1").arg(h[j]);
                }
                break;
                case SS_INT32:
                {
                    int32_T *I = (int32_T *)da[idx];
                    param_value = QString("%1").arg(I[j]);
                }
                break;
                case SS_UINT8:
                {
                    uint8_T *c = (uint8_T *)da[idx];
                    param_value = QString("%1").arg(c[j]);
                }
                break;
                case SS_BOOLEAN:
                {
                    boolean_T *c = (boolean_T *)da[idx];
                    param_value = QString("%1").arg(c[j]);
                }
                break;
                case SS_UINT16:
                {
                    uint16_T *h = (uint16_T *)da[idx];
                    param_value = QString("%1").arg(h[j]);
                }
                break;
                case SS_UINT32:
                {
                    uint32_T *I = (uint32_T *)da[idx];
                    param_value = QString("%1").arg(I[j]);
                }
                break;
                }
                QTableWidgetItem *newItem = new QTableWidgetItem(param_name);
                ui->tableWidget->setItem(cur, 0, newItem);
                newItem = new QTableWidgetItem(param_value);
                ui->tableWidget->setItem(cur++, 1, newItem);
                if (param_name == "X0")
                {
                    X0[0] = param_value.toDouble();
                }
                else if (param_name == "X0(1)")
                {
                    X0[1] = param_value.toDouble();
                }
                else if (param_name == "X0(2)")
                {
                    X0[2] = param_value.toDouble();
                }
                else if (param_name == "Att0")
                {
                    Att0[0] = param_value.toDouble();
                }
                else if (param_name == "Att0(1)")
                {
                    Att0[1] = param_value.toDouble();
                }
                else if (param_name == "Att0(2)")
                {
                    Att0[2] = param_value.toDouble();
                }
                else if (param_name == "V0")
                {
                    V0[0] = param_value.toDouble();
                }
                else if (param_name == "V0(1)")
                {
                    V0[1] = param_value.toDouble();
                }
                else if (param_name == "V0(2)")
                {
                    V0[2] = param_value.toDouble();
                }
                else if (param_name == "hground")
                {
                    hground = param_value.toDouble();
                }
            }
        }
        ui->tableWidget->setRowCount(cur);
    }
}


void MainWindow::setup_param(const QVector<QString> &head, const QVector<double> &onecase)
{
    const rtwCAPI_ModelMappingStaticInfo *sm = (*getmap_func)();
    if (sm)
    {
        //unsigned int n = rtwCAPI_GetNumBlockParametersFromStaticMap(sm);
        unsigned int m = rtwCAPI_GetNumModelParametersFromStaticMap(sm);
        rtwCAPI_ModelParameters const *prm = rtwCAPI_GetModelParametersFromStaticMap(sm);
        rtwCAPI_DataTypeMap const *dtm = rtwCAPI_GetDataTypeMapFromStaticMap(sm);
        rtwCAPI_DimensionMap const *dmm = rtwCAPI_GetDimensionMapFromStaticMap(sm);
        uint_T const *dam = rtwCAPI_GetDimensionArrayFromStaticMap(sm);
        rtwCAPI_ModelMappingInfo *MMI = &(rtmGetDataMapInfo(this->m).mmi);
        void **da = rtwCAPI_GetDataAddressMap(MMI);
        for (unsigned int i = 0; i < m; ++i)
        {
            const char *name = rtwCAPI_GetModelParameterName(prm, i);
            int idx = rtwCAPI_GetModelParameterAddrIdx(prm, i);
            uint16_t dti = rtwCAPI_GetModelParameterDataTypeIdx(prm, i);
            uint16_t dmi = rtwCAPI_GetModelParameterDimensionIdx(prm, i);
            uint8_t ss = rtwCAPI_GetDataTypeSLId(dtm, dti);
            uint16_t dai = rtwCAPI_GetDimArrayIndex(dmm, dmi);
            uint8_t nd = rtwCAPI_GetNumDims(dmm, dmi);
            uint16_t n = 1;
            for (int j = 0; j < nd; ++j)
            {
                n *= dam[dai + j];
            }
            int j = 0;
            for (auto i = head.begin(); i != head.end(); ++i, ++j)
            {
                QStringList s = i->split(QRegExp("[()]"));
                if (*i == name || ((s.size() >= 2) && (s.value(0) == name)))
                {
                    int target_idx = 0;
                    double target_value = onecase[j];
                    if (s.size() >= 2)
                    {
                        target_idx = s.value(1).toInt();
                    }
                    switch (ss)
                    {
                    case SS_DOUBLE:
                    {
                        double *d = (double *)da[idx];
                        d[target_idx] = target_value;
                    }
                    break;
                    case SS_SINGLE:
                    {
                        float *s = (float *)da[idx];
                        s[target_idx] = target_value;
                    }
                    break;
                    case SS_INT8:
                    {
                        int8_T *c = (int8_T *)da[idx];
                        c[target_idx] = target_value;
                    }
                    break;
                    case SS_INT16:
                    {
                        int16_T *h = (int16_T *)da[idx];
                        h[target_idx] = target_value;
                    }
                    break;
                    case SS_INT32:
                    {
                        int32_T *I = (int32_T *)da[idx];
                        I[target_idx] = target_value;
                    }
                    break;
                    case SS_UINT8:
                    {
                        uint8_T *c = (uint8_T *)da[idx];
                        c[target_idx] = target_value;
                    }
                    break;
                    case SS_BOOLEAN:
                    {
                        bool *c = (bool *)da[idx];
                        c[target_idx] = target_value >=0.5?true:false;
                    }
                    break;
                    case SS_UINT16:
                    {
                        uint16_T *h = (uint16_T *)da[idx];
                        h[target_idx] = target_value;
                    }
                    break;
                    case SS_UINT32:
                    {
                        uint32_T *I = (uint32_T *)da[idx];
                        I[target_idx] = target_value;
                    }
                    break;
                    }
                }
            }
        }
    }
}


MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent), ui(new Ui::MainWindow), times(1), idx(0)
{
    ui->setupUi(this);

    QLibrary *mdl = new QLibrary("SIL_win64.dll");
    if (mdl->isLoaded())
    {
        init_func = static_cast<SIL_initialize_ptr>(mdl->resolve("SIL_initialize"));
        step_func = static_cast<SIL_step_ptr>(mdl->resolve("SIL_step"));
        getmap_func = (SIL_GetCAPIStaticMap_ptr)(mdl->resolve("SIL_GetCAPIStaticMap"));
        u = (ExtU_SIL_T *)mdl->resolve("SIL_U");
        y = (ExtY_SIL_T *)mdl->resolve("SIL_Y");
        V0 = (double *)mdl->resolve("V0");
        m = (RT_MODEL_SIL_T *)mdl->resolve("SIL_M_");
    }
    else
    {
        init_func = &SIL_initialize;
        step_func = &SIL_step;
        getmap_func = &SIL_GetCAPIStaticMap;
        u = &SIL_U;
        y = &SIL_Y;
        V0 = ::V0;
        m = (RT_MODEL_SIL_T *)SIL_M;
    }

    ui->pushButton_start->setText(QString("Fail"));
    stage = 0;

    if (init_func && step_func && u && y && getmap_func)
    {
        SIL_InitializeDataMapInfo();
        scan_param();

        ui->lineEditLat->setText(QString("%1").arg(X0[0]));
        ui->lineEditLon->setText(QString("%1").arg(X0[1]));
        ui->lineEditAlt->setText(QString("%1").arg(hground));
        ui->lineEditHdg->setText(QString("%1").arg(Att0[2]*57.3));
        ui->lineEditSpeed->setText(QString("%1").arg(V0[0]));
        gear_height = X0[2] - hground;
        ui->lineEditAGL->setText(QString("%1").arg(gear_height));

        qgc_sock = new QUdpSocket(this);
        if (qgc_sock->bind(QHostAddress::LocalHost, 6061))
        {
            ui->pushButton_start->setText(QString("Start"));
            stage = 1;
        }
    }
}

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

void MainWindow::update200Hz(void)
{
    for (int j=0;j<times;++j)
    {
        u->data_len = 0u;
        if (qgc_sock->hasPendingDatagrams())
        {
            qint64 len = qgc_sock->readDatagram((char *)u->data_in, 255);
            if (len > 0)
            {
                u->data_len = static_cast<uint16_t>(len);
            }
        }

        for (int i = 0; i < 1; ++i)
        {
            (*step_func)();
        }

        if (y->seq != seq)
        {
            seq = y->seq;
            qgc_sock->writeDatagram((char *)y->buff, y->buff_len, QHostAddress::LocalHost, 6060);
        }
        if (y->rec_seq != seq_rec)
        {
            seq_rec = y->rec_seq;
            rec_file->write((const char *)y->rec_buff, y->rec_len);
        }
    }
    if (++idx >= 20)
    {
        idx = 0;
        ui->lineEditMass->setText(QString("%1").arg(y->mass,6,'f',2));
        ui->lineEditRPM->setText(QString("%1").arg(y->RPM,5,'f',0));
        ui->lineEditDel->setText(QString("%1").arg(y->de*57.3,6,'f',2));
        ui->lineEditDer->setText(QString("%1").arg(y->de*57.3,6,'f',2));
        ui->lineEditDal->setText(QString("%1").arg(y->da*-57.3,6,'f',2));
        ui->lineEditDar->setText(QString("%1").arg(y->da*57.3,6,'f',2));
        ui->lineEditDr->setText(QString("%1").arg(y->dr*57.3,6,'f',2));
        if (y->RPM > 44500 && u->launch_trigger == 0)
        {
            u->launch_trigger = 1.0;
            ui->pushButton_start->setText(QString("Flying"));
            ui->pushButtonLaunch->setText(QString("Flying"));
            ui->pushButton_start->setEnabled(false);
            ui->pushButtonLaunch->setEnabled(false);
            stage = 3;
            ui->tabWidget->setCurrentIndex(1);
        }
        if (ui->checkBoxTurbine->checkState() == Qt::Checked)
        {
            u->fault |= 1;
        }
        else
        {
            u->fault &= 0xFFFFFFFE;
        }
        if (ui->checkBoxDel->checkState() == Qt::Checked)
        {
            u->fault |= 2;
        }
        else
        {
            u->fault &= 0xFFFFFFFD;
        }
        if (ui->checkBoxDer->checkState() == Qt::Checked)
        {
            u->fault |= 4;
        }
        else
        {
            u->fault &= 0xFFFFFFFB;
        }
        if (ui->checkBoxDr->checkState() == Qt::Checked)
        {
            u->fault |= 8;
        }
        else
        {
            u->fault &= 0xFFFFFFF7;
        }
        if (ui->checkBoxDal->checkState() == Qt::Checked)
        {
            u->fault |= 0x10;
        }
        else
        {
            u->fault &= 0xFFFFFFEF;
        }
        if (ui->checkBoxDar->checkState() == Qt::Checked)
        {
            u->fault |= 0x20;
        }
        else
        {
            u->fault &= 0xFFFFFFDF;
        }
    }
}

void MainWindow::onFire()
{
    if (stage == 1)
    {
        QVector<QString> head;
        QVector<double> onecase;

        int n = ui->tableWidget->rowCount();
        for (int i=0;i<n;++i)
        {
            head.push_back(ui->tableWidget->item(i,0)->text());
            onecase.push_back(ui->tableWidget->item(i,1)->text().toDouble());
        }

        double val;
        bool ok;
        val = ui->lineEditLat->text().toDouble(&ok);
        if (ok)
        {
            head.push_back("X0");
            onecase.push_back(val);
        }

        val = ui->lineEditLon->text().toDouble(&ok);
        if (ok)
        {
            head.push_back("X0(1)");
            onecase.push_back(val);
        }

        double hground;
        val = ui->lineEditAlt->text().toDouble(&ok);
        if (ok)
        {
            hground = val;
            head.push_back("hground");
            onecase.push_back(hground);
            if (ui->radioButtonOnGround->isChecked())
            {
                head.push_back("X0(2)");
                onecase.push_back(hground+gear_height);
            }
        }

        val = ui->lineEditHdg->text().toDouble(&ok);
        if (ok)
        {
            head.push_back("Att0(2)");
            onecase.push_back(val/57.3);
        }

        if (ui->radioButtonInAir->isChecked())
        {
            head.push_back("Att0(1)");
            onecase.push_back(0);
            val = ui->lineEditSpeed->text().toDouble(&ok);
            if (ok)
            {
                head.push_back("V0");
                onecase.push_back(val);
            }
            val = ui->lineEditAGL->text().toDouble(&ok);
            if (ok)
            {
                head.push_back("X0(2)");
                onecase.push_back(hground+val);
            }
            head.push_back("force_sysauto_level");
            onecase.push_back(0);
        }
        else
        {
            head.push_back("force_sysauto_level");
            onecase.push_back(1);
        }

        setup_param(head, onecase);
        (*init_func)();

        QString filename = QString("sil")
                +QDateTime::currentDateTime().toString("yyyyMMddhhmmss")
                +".DAT";
        rec_file = new QFile(filename, this);
        rec_file->open(QFile::WriteOnly);

        seq = 0;
        seq_rec = 0;

        timer200Hz = new QTimer(this);
        connect(timer200Hz, SIGNAL(timeout()), this, SLOT(update200Hz()));
        timer200Hz->start(5);

        if (ui->radioButtonOnGround->isChecked())
        {
            stage = 2;
            ui->pushButton_start->setText(QString("Launch"));
            ui->pushButtonLaunch->setEnabled(true);
            ui->tabWidget->setCurrentIndex(1);
            u->fault = 0x0;
        }
        if (ui->radioButtonInAir->isChecked())
        {
            stage = 3;
            ui->pushButton_start->setText(QString("Flying"));
            ui->pushButtonLaunch->setText(QString("Flying"));
            ui->pushButton_start->setEnabled(false);
            ui->pushButtonLaunch->setEnabled(false);
            ui->tabWidget->setCurrentIndex(1);
            u->fault = 0x40;
        }
    }
    else if (stage == 2)
    {
        u->launch_trigger = 1.0;
        ui->pushButton_start->setText(QString("Flying"));
        ui->pushButtonLaunch->setText(QString("Flying"));
        ui->pushButton_start->setEnabled(false);
        ui->pushButtonLaunch->setEnabled(false);
        stage = 3;
        ui->tabWidget->setCurrentIndex(1);
    }
}

void MainWindow::onExport()
{
    QString filename = QFileDialog::getSaveFileName(this, tr("Save to"), QString(), tr("Text File (*.txt)"));
    if (!filename.isEmpty())
    {
        QFile data(filename);
        if (data.open(QFile::WriteOnly | QFile::Truncate)) {
            QTextStream out(&data);
            int n = ui->tableWidget->rowCount();
            for (int i=0;i<n;++i)
            {
                out << ui->tableWidget->item(i,0)->text() << ',';
                out << ui->tableWidget->item(i,1)->text() << '\n';
            }
            out << QString("X0") << ',' << ui->lineEditLat->text() << '\n';
            out << QString("X0(1)") << ',' << ui->lineEditLon->text() << '\n';
            out << QString("hground") << ',' << ui->lineEditAlt->text() << '\n';
            out << QString("Att0(2)") << ',' << ui->lineEditHdg->text().toDouble()/57.3 << '\n';
        }
    }
}

void MainWindow::onImport()
{
    QString filename = QFileDialog::getOpenFileName(this, tr("Save to"), QString(), tr("Text File (*.txt)"));
    if (!filename.isEmpty())
    {
        QFile data(filename);
        if (data.open(QFile::ReadOnly)) {
            QTextStream stream(&data);
            QString line;
            while (stream.readLineInto(&line)) {
              QStringList items = line.split(QString(","));
              int n = ui->tableWidget->rowCount();
              for (int i=0;i<n;++i)
              {
                  if (ui->tableWidget->item(i,0)->text() == items[0])
                  {
                      ui->tableWidget->item(i,1)->setText(items[1]);
                      break;
                  }
              }
            }
            {
                int n = ui->tableWidget->rowCount();
                for (int i=0;i<n;++i)
                {
                    if (ui->tableWidget->item(i,0)->text() == "X0")
                    {
                        ui->lineEditLat->setText(ui->tableWidget->item(i,1)->text());
                    }
                    else if (ui->tableWidget->item(i,0)->text() == "X0(1)")
                    {
                        ui->lineEditLon->setText(ui->tableWidget->item(i,1)->text());
                    }
                    else if (ui->tableWidget->item(i,0)->text() == "hground")
                    {
                        ui->lineEditAlt->setText(ui->tableWidget->item(i,1)->text());
                    }
                    else if (ui->tableWidget->item(i,0)->text() == "Att0(2)")
                    {
                        ui->lineEditHdg->setText(QString("%1").arg(ui->tableWidget->item(i,1)->text().toDouble()*57.3));
                    }
                }
            }
        }
    }

}

void MainWindow::onTimes(int i)
{
    times = i;
}

void MainWindow::onAG()
{
    if (ui->radioButtonOnGround->isChecked())
    {
        ui->lineEditSpeed->setEnabled(false);
        ui->lineEditAGL->setEnabled(false);
    }
    if (ui->radioButtonInAir->isChecked())
    {
        ui->lineEditSpeed->setEnabled(true);
        ui->lineEditAGL->setEnabled(true);
    }
}