/*=====================================================================

QGroundControl Open Source Ground Control Station

(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

This file is part of the QGROUNDCONTROL project

    QGROUNDCONTROL is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    QGROUNDCONTROL is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Configuration Window for Slugs' HIL Simulator
 *   @author Mariano Lizarraga <malife@gmail.com>
 *   @author Alejandro Molina <am.alex09@gmail.com>
 */


#include "SlugsHilSim.h"
#include "ui_SlugsHilSim.h"


SlugsHilSim::SlugsHilSim(QWidget *parent) :
    QWidget(parent),
    ui(new Ui::SlugsHilSim)
{
    ui->setupUi(this);

    rxSocket = new QUdpSocket(this);
    txSocket = new QUdpSocket(this);

    hilLink = NULL;

    connect(LinkManager::instance(), SIGNAL(newLink(LinkInterface*)), this, SLOT(addToCombo(LinkInterface*)));
    connect(ui->cb_mavlinkLinks, SIGNAL(currentIndexChanged(int)), this, SLOT(linkSelected(int)));
    connect(ui->bt_startHil, SIGNAL(clicked()), this, SLOT(putInHilMode()));
    connect(rxSocket, SIGNAL(readyRead()), this, SLOT(readDatagram()));

    linksAvailable.clear();

#ifdef MAVLINK_ENABLED_SLUGS
    memset(&tmpAirData, 0, sizeof(mavlink_air_data_t));
    memset(&tmpAttitudeData, 0, sizeof(mavlink_attitude_t));
    memset(&tmpGpsData, 0, sizeof(mavlink_gps_raw_t));
    memset(&tmpGpsTime, 0, sizeof(mavlink_gps_date_time_t));
    memset(&tmpLocalPositionData, 0, sizeof(mavlink_sensor_bias_t));
    memset(&tmpRawImuData, 0, sizeof(mavlink_raw_imu_t));
#endif

    foreach (LinkInterface* link, LinkManager::instance()->getLinks()) {
        addToCombo(link);
    }
}

SlugsHilSim::~SlugsHilSim()
{
    rxSocket->disconnectFromHost();
    delete ui;
}

void SlugsHilSim::addToCombo(LinkInterface* theLink)
{

    linksAvailable.insert(ui->cb_mavlinkLinks->count(),theLink);
    ui->cb_mavlinkLinks->addItem(theLink->getName());

    if (hilLink == NULL) {
        hilLink = theLink;
    }

}

void SlugsHilSim::putInHilMode(void)
{

    bool sw_enableControls = !(ui->bt_startHil->isChecked());
    QString  buttonCaption= ui->bt_startHil->isChecked()? "Stop Slugs HIL Mode": "Set Slugs in HIL Mode";

    if (ui->bt_startHil->isChecked()) {
        QMessageBox msgBox;
        msgBox.setIcon(QMessageBox::Critical);
        msgBox.setText("You are about to put SLUGS in HIL Mode.");
        msgBox.setInformativeText("It will stop reading the actual sensor readings. Do you wish to continue?");
        msgBox.setStandardButtons(QMessageBox::Yes | QMessageBox::No);
        msgBox.setDefaultButton(QMessageBox::No);

        if(msgBox.exec() == QMessageBox::Yes) {
            rxSocket->disconnectFromHost();
            rxSocket->bind(QHostAddress::Any, ui->ed_rxPort->text().toInt());
            //txSocket->bind(QHostAddress::Broadcast, ui->ed_txPort->text().toInt());

            ui->ed_ipAdress->setEnabled(sw_enableControls);
            ui->ed_rxPort->setEnabled(sw_enableControls);
            ui->ed_txPort->setEnabled(sw_enableControls);
            ui->cb_mavlinkLinks->setEnabled(sw_enableControls);

            ui->bt_startHil->setText(buttonCaption);

            activeUas->startHil();

        } else {
            ui->bt_startHil->setChecked(false);
        }
    } else {
        ui->ed_ipAdress->setEnabled(sw_enableControls);
        ui->ed_rxPort->setEnabled(sw_enableControls);
        ui->ed_txPort->setEnabled(sw_enableControls);
        ui->cb_mavlinkLinks->setEnabled(sw_enableControls);

        ui->bt_startHil->setText(buttonCaption);

        rxSocket->disconnectFromHost();
        activeUas->stopHil();
    }
}

void SlugsHilSim::readDatagram(void)
{
    static int count = 0;
    while (rxSocket->hasPendingDatagrams()) {
        QByteArray datagram;
        datagram.resize(rxSocket->pendingDatagramSize());
        QHostAddress sender;
        quint16 senderPort;

        rxSocket->readDatagram(datagram.data(), datagram.size(),
                               &sender, &senderPort);

        if (datagram.size() == 113) {
            processHilDatagram(&datagram);

            sendMessageToSlugs();

            commandDatagramToSimulink();
        }

        ui->ed_count->setText(QString::number(count++));
    }
}


void SlugsHilSim::activeUasSet(UASInterface* uas)
{

    if (uas != NULL) {
        activeUas = static_cast <UAS *>(uas);
    }
}


void SlugsHilSim::processHilDatagram(const QByteArray* datagram)
{
#ifdef MAVLINK_ENABLED_SLUGS
    unsigned char i = 0;


    tmpGpsTime.year  = datagram->at(i++);
    tmpGpsTime.month = datagram->at(i++);
    tmpGpsTime.day   = datagram->at(i++);
    tmpGpsTime.hour  = datagram->at(i++);
    tmpGpsTime.min   = datagram->at(i++);
    tmpGpsTime.sec   = datagram->at(i++);

    tmpGpsData.lat = getFloatFromDatagram(datagram, &i);
    tmpGpsData.lon = getFloatFromDatagram(datagram, &i);
    tmpGpsData.alt = getFloatFromDatagram(datagram, &i);

    tmpGpsData.hdg = getUint16FromDatagram(datagram, &i);
    tmpGpsData.v   = getUint16FromDatagram(datagram, &i);

    tmpGpsData.eph = getUint16FromDatagram(datagram, &i);
    tmpGpsData.fix_type = datagram->at(i++);
    tmpGpsTime.visSat  = datagram->at(i++);
    i++;

    tmpAirData.dynamicPressure= getFloatFromDatagram(datagram, &i);
    tmpAirData.staticPressure= getFloatFromDatagram(datagram, &i);
    tmpAirData.temperature= getUint16FromDatagram(datagram, &i);

    // TODO Salto en el Datagrama
    i=i+8;

    tmpRawImuData.xgyro = getUint16FromDatagram(datagram, &i);
    tmpRawImuData.ygyro = getUint16FromDatagram(datagram, &i);
    tmpRawImuData.zgyro = getUint16FromDatagram(datagram, &i);
    tmpRawImuData.xacc = getUint16FromDatagram(datagram, &i);
    tmpRawImuData.yacc = getUint16FromDatagram(datagram, &i);
    tmpRawImuData.zacc = getUint16FromDatagram(datagram, &i);
    tmpRawImuData.xmag = getUint16FromDatagram(datagram, &i);
    tmpRawImuData.ymag = getUint16FromDatagram(datagram, &i);
    tmpRawImuData.zmag = getUint16FromDatagram(datagram, &i);

    tmpAttitudeData.roll = getFloatFromDatagram(datagram, &i);
    tmpAttitudeData.pitch = getFloatFromDatagram(datagram, &i);
    tmpAttitudeData.yaw = getFloatFromDatagram(datagram, &i);

    tmpAttitudeData.rollspeed = getFloatFromDatagram(datagram, &i);
    tmpAttitudeData.pitchspeed = getFloatFromDatagram(datagram, &i);
    tmpAttitudeData.yawspeed = getFloatFromDatagram(datagram, &i);

    // TODO Crear Paquete SYNC TIME
    i=i+2;

    tmpLocalPositionData.x = getFloatFromDatagram(datagram, &i);
    tmpLocalPositionData.y = getFloatFromDatagram(datagram, &i);
    tmpLocalPositionData.z = getFloatFromDatagram(datagram, &i);
    tmpLocalPositionData.vx = getFloatFromDatagram(datagram, &i);
    tmpLocalPositionData.vy = getFloatFromDatagram(datagram, &i);
    tmpLocalPositionData.vz = getFloatFromDatagram(datagram, &i);

    // TODO: this is legacy of old HIL datagram. Need to remove from Simulink model
    i++;

    ui->ed_1->setText(QString::number(tmpRawImuData.xacc));
    ui->ed_2->setText(QString::number(tmpRawImuData.yacc));
    ui->ed_3->setText(QString::number(tmpRawImuData.zacc));

    ui->tbA->setText(QString::number(tmpRawImuData.xgyro));
    ui->tbB->setText(QString::number(tmpRawImuData.ygyro));
    ui->tbC->setText(QString::number(tmpRawImuData.zgyro));

#else
    Q_UNUSED(datagram);
#endif
}

float SlugsHilSim::getFloatFromDatagram (const QByteArray* datagram, unsigned char * i)
{
    tFloatToChar tmpF2C;

    tmpF2C.chData[0] = datagram->at((*i)++);
    tmpF2C.chData[1] = datagram->at((*i)++);
    tmpF2C.chData[2] = datagram->at((*i)++);
    tmpF2C.chData[3] = datagram->at((*i)++);

    return tmpF2C.flData;
}

uint16_t SlugsHilSim::getUint16FromDatagram (const QByteArray* datagram, unsigned char * i)
{
    tUint16ToChar tmpU2C;

    tmpU2C.chData[0] = datagram->at((*i)++);
    tmpU2C.chData[1] = datagram->at((*i)++);

    return tmpU2C.uiData;

}

void SlugsHilSim::linkSelected(int cbIndex)
{
#ifdef MAVLINK_ENABLED_SLUGS
    // HIL code to go here...
    //hilLink = linksAvailable
    // FIXME Mariano

    hilLink =linksAvailable.value(cbIndex);

#else
    Q_UNUSED(cbIndex)
#endif
}

void SlugsHilSim::sendMessageToSlugs()
{
#ifdef MAVLINK_ENABLED_SLUGS
    mavlink_message_t msg;

    mavlink_msg_local_position_encode(MG::SYSTEM::ID,
                                      MG::SYSTEM::COMPID,
                                      &msg,
                                      &tmpLocalPositionData);
    activeUas->sendMessage(hilLink, msg);
    memset(&msg, 0, sizeof(mavlink_message_t));

    mavlink_msg_attitude_encode(MG::SYSTEM::ID,
                                MG::SYSTEM::COMPID,
                                &msg,
                                &tmpAttitudeData);
    activeUas->sendMessage(hilLink, msg);
    memset(&msg, 0, sizeof(mavlink_message_t));

    mavlink_msg_raw_imu_encode(MG::SYSTEM::ID,
                               MG::SYSTEM::COMPID,
                               &msg,
                               &tmpRawImuData);
    activeUas->sendMessage(hilLink, msg);
    memset(&msg, 0, sizeof(mavlink_message_t));

    mavlink_msg_air_data_encode(MG::SYSTEM::ID,
                                MG::SYSTEM::COMPID,
                                &msg,
                                &tmpAirData);
    activeUas->sendMessage(hilLink, msg);
    memset(&msg, 0, sizeof(mavlink_message_t));

    mavlink_msg_gps_raw_encode(MG::SYSTEM::ID,
                               MG::SYSTEM::COMPID,
                               &msg,
                               &tmpGpsData);
    activeUas->sendMessage(hilLink, msg);
    memset(&msg, 0, sizeof(mavlink_message_t));

    mavlink_msg_gps_date_time_encode(MG::SYSTEM::ID,
                                     MG::SYSTEM::COMPID,
                                     &msg,
                                     &tmpGpsTime);
    activeUas->sendMessage(hilLink, msg);
    memset(&msg, 0, sizeof(mavlink_message_t));
#endif
}


void SlugsHilSim::commandDatagramToSimulink()
{
#ifdef MAVLINK_ENABLED_SLUGS
    //mavlink_pwm_commands_t* pwdC = (static_cast<SlugsMAV*>(activeUas))->getPwmCommands();

    //mavlink_pwm_commands_t* pwdC;

//    if(pwdC != NULL){
//    }

    QByteArray data;
    data.resize(22);

    unsigned char i=0;
    setUInt16ToDatagram(data, &i, 1);//pwdC->dt_c);
    setUInt16ToDatagram(data, &i, 2);//pwdC->dla_c);
    setUInt16ToDatagram(data, &i, 3);//pwdC->dra_c);
    setUInt16ToDatagram(data, &i, 4);//pwdC->dr_c);
    setUInt16ToDatagram(data, &i, 5);//pwdC->dle_c);
    setUInt16ToDatagram(data, &i, 6);//pwdC->dre_c);
    setUInt16ToDatagram(data, &i, 7);//pwdC->dlf_c);
    setUInt16ToDatagram(data, &i, 8);//pwdC->drf_c);
    setUInt16ToDatagram(data, &i, 9);//pwdC->aux1);
    setUInt16ToDatagram(data, &i, 10);//pwdC->aux2);
    setUInt16ToDatagram(data, &i, 11);//value default

    txSocket->writeDatagram(data, QHostAddress::Broadcast, ui->ed_txPort->text().toInt());
#endif
}

void SlugsHilSim::setUInt16ToDatagram(QByteArray& datagram, unsigned char* pos, uint16_t value)
{
    tUint16ToChar tmpUnion;
    tmpUnion.uiData= value;

    datagram[(*pos)++]= tmpUnion.chData[0];
    datagram[(*pos)++]= tmpUnion.chData[1];
}