/**
 ******************************************************************************
 *
 * @file       aerosimrc.cpp
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2012.
 * @addtogroup GCSPlugins GCS Plugins
 * @{
 * @addtogroup HITLPlugin HITLv2 Plugin
 * @{
 * @brief The Hardware In The Loop plugin version 2
 *****************************************************************************/
/*
 * This program 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.
 *
 * This program 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 this program; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 */

#include "aerosimrcsimulator.h"
#include <extensionsystem/pluginmanager.h>
#include <coreplugin/icore.h>
#include <coreplugin/threadmanager.h>

AeroSimRCSimulator::AeroSimRCSimulator(const SimulatorSettings &params)
    : Simulator(params)
{
    udpCounterASrecv = 0;
}

AeroSimRCSimulator::~AeroSimRCSimulator()
{}

bool AeroSimRCSimulator::setupProcess()
{
    QMutexLocker locker(&lock);

    return true;
}

void AeroSimRCSimulator::setupUdpPorts(const QString &host, int inPort, int outPort)
{
    Q_UNUSED(outPort)
    if (inSocket->bind(QHostAddress(host), inPort)) {
        emit processOutput("Successfully bound to address " + host + ", port " + QString::number(inPort) + "\n");
    } else {
        emit processOutput("Cannot bind to address " + host + ", port " + QString::number(inPort) + "\n");
    }
}

void AeroSimRCSimulator::transmitUpdate()
{
    // read actuator output
    ActuatorCommand::DataFields actCmdData;

    actCmdData = actCommand->getData();
    float channels[10];
    for (int i = 0; i < 10; ++i) {
        qint16 ch = actCmdData.Channel[i];
        float out = -1.0;
        if (ch >= 1000 && ch <= 2000) {
            ch -= 1000;
            out = ((float)ch / 500.0) - 1.0;
        }
        channels[i] = out;
    }

    ActuatorDesired::DataFields actData;
    FlightStatus::DataFields flightStatusData    = flightStatus->getData();
    ManualControlCommand::DataFields manCtrlData = manCtrlCommand->getData();
    float roll     = -1;
    float pitch    = -1;
    float yaw      = -1;
    float throttle = -1;

    if (flightStatusData.FlightMode == FlightStatus::FLIGHTMODE_MANUAL) {
        // Read joystick input
        if (flightStatusData.Armed == FlightStatus::ARMED_ARMED) {
            // Note: Pitch sign is reversed in FG ?
            roll     = manCtrlData.Roll;
            pitch    = -manCtrlData.Pitch;
            yaw      = manCtrlData.Yaw;
            throttle = manCtrlData.Throttle;
        }
    } else {
        // Read ActuatorDesired from autopilot
        actData  = actDesired->getData();

        roll     = actData.Roll;
        pitch    = -actData.Pitch;
        yaw      = actData.Yaw;
        throttle = (actData.Throttle * 2.0) - 1.0;
    }
    channels[0] = roll;
    channels[1] = pitch;
    if (throttle < -1) {
        throttle = -1;
    }
    channels[2] = throttle;
    channels[3] = yaw;

    // read flight status
    quint8 armed;
    quint8 mode;
    armed = flightStatusData.Armed;
    mode  = flightStatusData.FlightMode;

    QByteArray data;
    // 50 - current size of values, 4(quint32) + 10*4(float) + 2(quint8) + 4(quint32)
    data.resize(50);
    QDataStream stream(&data, QIODevice::WriteOnly);
    stream.setFloatingPointPrecision(QDataStream::SinglePrecision);
    stream << quint32(0x52434D44); // magic header, "RCMD"
    for (int i = 0; i < 10; ++i) {
        stream << channels[i]; // channels
    }
    stream << armed << mode; // flight status
    stream << udpCounterASrecv; // packet counter

    if (outSocket->writeDatagram(data, QHostAddress(settings.remoteAddress), settings.outPort) == -1) {
        qDebug() << "write failed: " << outSocket->errorString();
    }

#ifdef DBG_TIMERS
    static int cntTX = 0;
    if (cntTX >= 100) {
        qDebug() << "TX=" << 1000.0 * 100 / timeTX.restart();
        cntTX = 0;
    } else {
        ++cntTX;
    }
#endif
}

void AeroSimRCSimulator::processUpdate(const QByteArray &data)
{
    // check size
    if (data.size() > 192) {
        qDebug() << "!!! big datagram: " << data.size();
        return;
    }

    QByteArray buf = data;
    QDataStream stream(&buf, QIODevice::ReadOnly);
    stream.setFloatingPointPrecision(QDataStream::SinglePrecision);

    // check magic header
    quint32 magic;
    stream >> magic;
    if (magic != 0x4153494D) { // "AERO"
        qDebug() << "wrong magic: " << magic << ", correct: " << quint32(0x4153494D);
        return;
    }

#define AEROSIM_RCCHANNEL_NUMELEM 8
    float delT,
          homeX, homeY, homeZ,
          WpHX, WpHY, WpLat, WpLon,
          posX, posY, posZ, // world
          velX, velY, velZ, // world
          angX, angY, angZ, // model
          accX, accY, accZ; // model
    qreal lat, lon;
    float agl, // world
          yaw, pitch, roll, // model
          volt, curr, cons,
          rx, ry, rz, fx, fy, fz, ux, uy, uz, // matrix
          ch[AEROSIM_RCCHANNEL_NUMELEM];

    stream >> delT;
    stream >> homeX >> homeY >> homeZ;
    stream >> WpHX >> WpHY >> WpLat >> WpLon;
    stream >> posX >> posY >> posZ;
    stream >> velX >> velY >> velZ;
    stream >> angX >> angY >> angZ;
    stream >> accX >> accY >> accZ;
    stream >> lat >> lon >> agl;
    stream >> yaw >> pitch >> roll;
    stream >> volt >> curr >> cons;
    stream >> rx >> ry >> rz >> fx >> fy >> fz >> ux >> uy >> uz;
    stream >> ch[0] >> ch[1] >> ch[2] >> ch[3] >> ch[4] >> ch[5] >> ch[6] >> ch[7];
    stream >> udpCounterASrecv;

    Output2Hardware out;
    memset(&out, 0, sizeof(Output2Hardware));


    out.delT = delT;

    /*************************************************************************************/
    for (int i = 0; i < AEROSIM_RCCHANNEL_NUMELEM; i++) {
        out.rc_channel[i] = ch[i]; // Elements in rc_channel are between -1 and 1
    }

    /**********************************************************************************************/
    QMatrix4x4 mat;
    mat = QMatrix4x4(fy, fx, -fz, 0.0, // model matrix
                     ry, rx, -rz, 0.0, // (X,Y,Z) -> (+Y,+X,-Z)
                     -uy, -ux, uz, 0.0,
                     0.0, 0.0, 0.0, 1.0);
    mat.optimize();

    QQuaternion quat; // model quat
    asMatrix2Quat(mat, quat);

    /*************************************************************************************/
    // rotate gravity
    QVector3D acc = QVector3D(accY, accX, -accZ); // accel (X,Y,Z) -> (+Y,+X,-Z)
    QVector3D gee = QVector3D(0.0, 0.0, -GEE);
    QQuaternion qWorld = quat.conjugate();
    gee  = qWorld.rotatedVector(gee);
    acc += gee;

    out.rollRate  = angY * RAD2DEG;       // gyros (X,Y,Z) -> (+Y,+X,-Z)
    out.pitchRate = angX * RAD2DEG;
    out.yawRate   = angZ * -RAD2DEG;

    out.accX = acc.x();
    out.accY = acc.y();
    out.accZ = acc.z();

    /*************************************************************************************/
    QVector3D rpy; // model roll, pitch, yaw
    asMatrix2RPY(mat, rpy);

    out.roll    = rpy.x();
    out.pitch   = rpy.y();
    out.heading = rpy.z();


    /**********************************************************************************************/
    out.altitude    = posZ;
    out.agl = posZ;
    out.heading     = yaw * RAD2DEG;
    out.latitude    = lat * 10e6;
    out.longitude   = lon * 10e6;
    out.groundspeed = qSqrt(velX * velX + velY * velY);

    /**********************************************************************************************/
    out.dstN        = posY;
    out.dstE        = posX;
    out.dstD        = -posZ;

    out.velNorth    = velY;
    out.velEast     = velX;
    out.velDown     = -velZ;

    out.voltage     = volt;
    out.current     = curr;
    out.consumption = cons * 1000.0;

    updateUAVOs(out);

#ifdef DBG_TIMERS
    static int cntRX = 0;
    if (cntRX >= 100) {
        qDebug() << "RX=" << 1000.0 * 100 / timeRX.restart();
        cntRX = 0;
    } else {
        ++cntRX;
    }
#endif
}

// transfomations

void AeroSimRCSimulator::asMatrix2Quat(const QMatrix4x4 &m, QQuaternion &q)
{
    qreal w, x, y, z;

    // w always >= 0
    w = qSqrt(qMax(0.0, 1.0 + m(0, 0) + m(1, 1) + m(2, 2))) / 2.0;
    x = qSqrt(qMax(0.0, 1.0 + m(0, 0) - m(1, 1) - m(2, 2))) / 2.0;
    y = qSqrt(qMax(0.0, 1.0 - m(0, 0) + m(1, 1) - m(2, 2))) / 2.0;
    z = qSqrt(qMax(0.0, 1.0 - m(0, 0) - m(1, 1) + m(2, 2))) / 2.0;

    x = copysign(x, (m(1, 2) - m(2, 1)));
    y = copysign(y, (m(2, 0) - m(0, 2)));
    z = copysign(z, (m(0, 1) - m(1, 0)));

    q.setScalar(w);
    q.setX(x);
    q.setY(y);
    q.setZ(z);
}

void AeroSimRCSimulator::asMatrix2RPY(const QMatrix4x4 &m, QVector3D &rpy)
{
    qreal roll, pitch, yaw;

    if (qFabs(m(0, 2)) > 0.998) {
        // ~86.3°, gimbal lock
        roll  = 0.0;
        pitch = copysign(M_PI_2, -m(0, 2));
        yaw   = qAtan2(-m(1, 0), m(1, 1));
    } else {
        roll  = qAtan2(m(1, 2), m(2, 2));
        pitch = qAsin(-m(0, 2));
        yaw   = qAtan2(m(0, 1), m(0, 0));
    }

    rpy.setX(roll * RAD2DEG);
    rpy.setY(pitch * RAD2DEG);
    rpy.setZ(yaw * RAD2DEG);
}