/**
 ******************************************************************************
 *
 * @file       simulator.h
 * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
 * @addtogroup GCSPlugins GCS Plugins
 * @{
 * @addtogroup HITLPlugin HITL Plugin
 * @{
 * @brief The Hardware In The Loop plugin
 *****************************************************************************/
/*
 * 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
 */

#ifndef ISIMULATOR_H
#define ISIMULATOR_H

#include "qscopedpointer.h"
#include "uavobjectmanager.h"

#include "accelstate.h"
#include "actuatorcommand.h"
#include "actuatordesired.h"
#include "airspeedstate.h"
#include "attitudestate.h"
#include "attitudesettings.h"
#include "barosensor.h"
#include "flightbatterystate.h"
#include "flightstatus.h"
#include "gcsreceiver.h"
#include "gcstelemetrystats.h"
#include "gpspositionsensor.h"
#include "gpsvelocitysensor.h"
#include "groundtruth.h"
#include "gyrostate.h"
#include "homelocation.h"
#include "manualcontrolcommand.h"
#include "positionstate.h"
#include "sonaraltitude.h"
#include "velocitystate.h"

#include "utils/coordinateconversions.h"

#include <QObject>
#include <QUdpSocket>
#include <QTime>
#include <QTimer>
#include <QProcess>
#include <qmath.h>

/**
 * just imagine this was a class without methods and all public properties
 */
typedef struct _FLIGHT_PARAM {
    // time
    float T;
    float dT;
    unsigned int i;

    // speeds
    float cas; // Calibrated airspeed
    float tas; // True airspeed
    float groundspeed;

    // position (absolute)
    float X;
    float Y;
    float Z;

    // speed (absolute)
    float dX;
    float dY;
    float dZ;

    // acceleration (absolute)
    float ddX;
    float ddY;
    float ddZ;

    // angle
    float azimuth;
    float pitch;
    float roll;

    // rotation speed
    float dAzimuth;
    float dPitch;
    float dRoll;
} FLIGHT_PARAM;

struct AirParameters {
    float groundDensity;
    float groundTemp;
    float seaLevelPress;
    float tempLapseRate;
    float univGasConstant;
    float dryAirConstant;
    float relativeHumidity; // [%]
    float M; // Molar mass
};

typedef struct _CONNECTION {
    QString simulatorId;
    QString binPath;
    QString dataPath;
    QString hostAddress;
    QString remoteAddress;
    int     outPort;
    int     inPort;
    bool    startSim;
    bool    addNoise;
    QString latitude;
    QString longitude;

// bool homeLocation;

    bool    attRawEnabled;
    quint8  attRawRate;

    bool    attStateEnabled;
    bool    attActHW;
    bool    attActSim;
    bool    attActCalc;

    bool    baroSensorEnabled;
    quint16 baroAltRate;

    bool    groundTruthEnabled;
    quint16 groundTruthRate;

    bool    gpsPositionEnabled;
    quint16 gpsPosRate;

    bool    inputCommand;
    bool    gcsReceiverEnabled;
    bool    manualControlEnabled;
    quint16 minOutputPeriod;

    bool    airspeedStateEnabled;
    quint16 airspeedStateRate;
} SimulatorSettings;


struct Output2Hardware {
    float latitude;
    float longitude;
    float altitude;
    float agl; // [m]
    float heading;
    float groundspeed; // [m/s]
    float calibratedAirspeed; // [m/s]
    float trueAirspeed; // [m/s]
    float angleOfAttack;
    float angleOfSlip;
    float roll;
    float pitch;
    float pressure;
    float temperature;
    float velNorth; // [m/s]
    float velEast; // [m/s]
    float velDown; // [m/s]
    float dstN; // [m]
    float dstE; // [m]
    float dstD; // [m]
    float accX; // [m/s^2]
    float accY; // [m/s^2]
    float accZ; // [m/s^2]
    float rollRate; // [deg/s]
    float pitchRate; // [deg/s]
    float yawRate; // [deg/s]
    float delT; // [s]

    float rc_channel[GCSReceiver::CHANNEL_NUMELEM]; // Elements in rc_channel are between -1 and 1


    float rollDesired;
    float pitchDesired;
    float yawDesired;
    float throttleDesired;

    float voltage;
    float current;
    float consumption;
};

// struct Output2Simulator{
// float roll;
// float pitch;
// float yaw;
// float throttle;

// float ailerons;
// float rudder;
// float elevator;
// float motor;
// };

class Simulator : public QObject {
    Q_OBJECT

public:
    Simulator(const SimulatorSettings & params);
    virtual ~Simulator();

    bool isAutopilotConnected() const
    {
        return autopilotConnectionStatus;
    }
    bool isSimulatorConnected() const
    {
        return simConnectionStatus;
    }
    QString Name() const
    {
        return name;
    }
    void setName(QString str)
    {
        name = str;
    }

    QString SimulatorId() const
    {
        return simulatorId;
    }
    void setSimulatorId(QString str)
    {
        simulatorId = str;
    }

    float airDensityFromAltitude(float alt, AirParameters air, float gravity);
    float airPressureFromAltitude(float alt, AirParameters air, float gravity);
    float cas2tas(float CAS, float alt, AirParameters air, float gravity);
    float tas2cas(float TAS, float alt, AirParameters air, float gravity);


    static bool IsStarted()
    {
        return isStarted;
    }
    static void setStarted(bool val)
    {
        isStarted = val;
    }
    static QStringList & Instances()
    {
        return Simulator::instances;
    }
    static void setInstance(const QString & str)
    {
        Simulator::instances.append(str);
    }

    virtual void stopProcess() {}
    virtual void setupUdpPorts(const QString & host, int inPort, int outPort)
    {
        Q_UNUSED(host) Q_UNUSED(inPort) Q_UNUSED(outPort)
    }

    void resetInitialHomePosition();
    void updateUAVOs(Output2Hardware out);

    AirParameters getAirParameters();
    void setAirParameters(AirParameters airParameters);

signals:
    void autopilotConnected();
    void autopilotDisconnected();
    void simulatorConnected();
    void simulatorDisconnected();
    void processOutput(QString str);
    void deleteSimProcess();
    void myStart();
public slots:
    Q_INVOKABLE virtual bool setupProcess()
    {
        return true;
    }
private slots:
    void onStart();
    // void transmitUpdate();
    void receiveUpdate();
    void onAutopilotConnect();
    void onAutopilotDisconnect();
    void onSimulatorConnectionTimeout();
    void telStatsUpdated(UAVObject *obj);
    Q_INVOKABLE void onDeleteSimulator(void);

    virtual void transmitUpdate() = 0;
    virtual void processUpdate(const QByteArray & data) = 0;

protected:
    static const float GEE;
    static const float FT2M;
    static const float KT2MPS;
    static const float INHG2KPA;
    static const float FPS2CMPS;
    static const float DEG2RAD;
    static const float RAD2DEG;

    QProcess *simProcess;
    QTime *time;
    QUdpSocket *inSocket; // (new QUdpSocket());
    QUdpSocket *outSocket;

    ActuatorCommand *actCommand;
    ActuatorDesired *actDesired;
    ManualControlCommand *manCtrlCommand;
    FlightStatus *flightStatus;
    FlightBatteryState *flightBatt;
    BaroSensor *baroAlt;
    AirspeedState *airspeedState;
    AttitudeState *attState;
    AttitudeSettings *attSettings;
    VelocityState *velState;
    GPSPositionSensor *gpsPos;
    GPSVelocitySensor *gpsVel;
    PositionState *posState;
    HomeLocation *posHome;
    AccelState *accelState;
    GyroState *gyroState;
    GCSTelemetryStats *telStats;
    GCSReceiver *gcsReceiver;
    GroundTruth *groundTruth;

    SimulatorSettings settings;

    FLIGHT_PARAM current;
    FLIGHT_PARAM old;
    QMutex lock;

private:
    bool once;
    float initN;
    float initE;
    float initD;

    int updatePeriod;
    int simTimeout;
    volatile bool autopilotConnectionStatus;
    volatile bool simConnectionStatus;
    QTimer *txTimer;
    QTimer *simTimer;

    QTime attRawTime;
    QTime gpsPosTime;
    QTime groundTruthTime;
    QTime baroAltTime;
    QTime battTime;
    QTime gcsRcvrTime;
    QTime airspeedStateTime;

    QString name;
    QString simulatorId;
    volatile static bool isStarted;
    static QStringList instances;
    // QList<QScopedPointer<UAVDataObject> > requiredUAVObjects;
    void setupOutputObject(UAVObject *obj, quint32 updatePeriod);
    void setupInputObject(UAVObject *obj, quint32 updatePeriod);
    void setupWatchedObject(UAVObject *obj, quint32 updatePeriod);
    void setupObjects();

    AirParameters airParameters;
};


class SimulatorCreator {
public:
    SimulatorCreator(QString id, QString descr) :
        classId(id),
        description(descr)
    {}
    virtual ~SimulatorCreator() {}

    QString ClassId() const
    {
        return classId;
    }
    QString Description() const
    {
        return description;
    }

    virtual Simulator *createSimulator(const SimulatorSettings & params) = 0;

private:
    QString classId;
    QString description;
};

#endif // ISIMULATOR_H