/*
   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, see <http://www.gnu.org/licenses/>.
 */
/*
  simulate VectorNav serial AHRS
*/

#include "SIM_VectorNav.h"
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>

using namespace SITL;

VectorNav::VectorNav() :
    SerialDevice::SerialDevice()
{
}

struct PACKED VN_packet1 {
    uint64_t timeStartup;
    uint64_t timeGPS;
    float uncompAccel[3];
    float uncompAngRate[3];
    float pressure;
    float mag[3];
    float accel[3];
    float gyro[3];
    uint16_t sensSat;
    uint16_t AHRSStatus;
    float ypr[3];
    float quaternion[4];
    float linAccBody[3];
    float yprU[3];
    uint16_t INSStatus;
    double positionLLA[3];
    float velNED[3];
    float posU;
    float velU;
};

struct PACKED VN_packet2 {
    uint64_t timeGPS;
    float temp;
    uint8_t numGPS1Sats;
    uint8_t GPS1Fix;
    double GPS1posLLA[3];
    float GPS1velNED[3];
    float GPS1DOP[7];
    uint8_t numGPS2Sats;
    uint8_t GPS2Fix;
    float GPS2DOP[7];
};

#define VN_PKT1_HEADER { 0xfa, 0x35, 0x03, 0x00, 0x2c, 0x0f, 0x47, 0x01, 0x13, 0x06 }
#define VN_PKT2_HEADER { 0xfa, 0x4e, 0x02, 0x00, 0x10, 0x00, 0xb8, 0x20, 0x18, 0x20 }

/*
  get timeval using simulation time
 */
static void simulation_timeval(struct timeval *tv)
{
    uint64_t now = AP_HAL::micros64();
    static uint64_t first_usec;
    static struct timeval first_tv;
    if (first_usec == 0) {
        first_usec = now;
        first_tv.tv_sec = AP::sitl()->start_time_UTC;
    }
    *tv = first_tv;
    tv->tv_sec += now / 1000000ULL;
    uint64_t new_usec = tv->tv_usec + (now % 1000000ULL);
    tv->tv_sec += new_usec / 1000000ULL;
    tv->tv_usec = new_usec % 1000000ULL;
}

void VectorNav::send_packet1(void)
{
    const auto &fdm = _sitl->state;

    struct VN_packet1 pkt {};

    struct timeval tv;
    simulation_timeval(&tv);

    if (start_us == 0) {
        start_us = tv.tv_usec * 1000;
    }

    pkt.timeStartup = start_us;
    pkt.timeGPS = tv.tv_usec * 1000;
    pkt.uncompAccel[0] = fdm.xAccel;
    pkt.uncompAccel[1] = fdm.yAccel;
    pkt.uncompAccel[2] = fdm.zAccel;
    const float gyro_noise = 0.05;
    pkt.uncompAngRate[0] = radians(fdm.rollRate + gyro_noise * rand_float());
    pkt.uncompAngRate[1] = radians(fdm.pitchRate + gyro_noise * rand_float());
    pkt.uncompAngRate[2] = radians(fdm.yawRate + gyro_noise * rand_float());

    float sigma, delta, theta;
    AP_Baro::SimpleAtmosphere(fdm.altitude * 0.001f, sigma, delta, theta);
    pkt.pressure = SSL_AIR_PRESSURE * delta * 0.001 + rand_float() * 0.01;

    pkt.mag[0] = fdm.bodyMagField.x*0.001;
    pkt.mag[1] = fdm.bodyMagField.y*0.001;
    pkt.mag[2] = fdm.bodyMagField.z*0.001;

    pkt.accel[0] = fdm.xAccel;
    pkt.accel[1] = fdm.yAccel;
    pkt.accel[2] = fdm.zAccel;
    pkt.gyro[0] = radians(fdm.rollRate + rand_float() * gyro_noise);
    pkt.gyro[1] = radians(fdm.pitchRate + rand_float() * gyro_noise);
    pkt.gyro[2] = radians(fdm.yawRate + rand_float() * gyro_noise);
    
    // pkt.sensSat = ???
    // pkt.AHRSStatus = ???

    pkt.ypr[0] = fdm.yawDeg;
    pkt.ypr[1] = fdm.pitchDeg;
    pkt.ypr[2] = fdm.rollDeg;

    pkt.quaternion[0] = fdm.quaternion.q2;
    pkt.quaternion[1] = fdm.quaternion.q3;
    pkt.quaternion[2] = fdm.quaternion.q4;
    pkt.quaternion[3] = fdm.quaternion.q1;

    pkt.linAccBody[0] = fdm.xAccel;
    pkt.linAccBody[1] = fdm.yAccel;
    pkt.linAccBody[2] = fdm.zAccel;

    // pkt.yprU[3] = attitude uncertainty

    // pkt.INSStatus = ??

    pkt.positionLLA[0] = fdm.latitude;
    pkt.positionLLA[1] = fdm.longitude;
    pkt.positionLLA[2] = fdm.altitude;
    pkt.velNED[0] = fdm.speedN;
    pkt.velNED[1] = fdm.speedE;
    pkt.velNED[2] = fdm.speedD;
    pkt.posU = 0.5;
    pkt.velU = 0.25;

    const uint8_t header[] VN_PKT1_HEADER;

    write_to_autopilot((char *)&header, sizeof(header));
    write_to_autopilot((char *)&pkt, sizeof(pkt));

    uint16_t crc = crc16_ccitt(&header[1], sizeof(header)-1, 0);
    crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc);
    uint16_t crc2;
    swab(&crc, &crc2, 2);

    write_to_autopilot((char *)&crc2, sizeof(crc2));
}

void VectorNav::send_packet2(void)
{
    const auto &fdm = _sitl->state;

    struct VN_packet2 pkt {};

    struct timeval tv;
    simulation_timeval(&tv);

    pkt.timeGPS = tv.tv_usec * 1000ULL;
    pkt.temp = 23.5;
    pkt.numGPS1Sats = 19;
    pkt.GPS1Fix = 3;
    pkt.GPS1posLLA[0] = fdm.latitude;
    pkt.GPS1posLLA[1] = fdm.longitude;
    pkt.GPS1posLLA[2] = fdm.altitude;
    pkt.GPS1velNED[0] = fdm.speedN;
    pkt.GPS1velNED[1] = fdm.speedE;
    pkt.GPS1velNED[2] = fdm.speedD;
    // pkt.GPS1DOP =
    pkt.numGPS2Sats = 18;
    pkt.GPS2Fix = 3;
    // pkt.GPS2DOP =

    const uint8_t header[] VN_PKT2_HEADER;

    write_to_autopilot((char *)&header, sizeof(header));
    write_to_autopilot((char *)&pkt, sizeof(pkt));

    uint16_t crc = crc16_ccitt(&header[1], sizeof(header)-1, 0);
    crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc);

    uint16_t crc2;
    swab(&crc, &crc2, 2);

    write_to_autopilot((char *)&crc2, sizeof(crc2));
}

/*
  send VectorNav data
 */
void VectorNav::update(void)
{
    if (!init_sitl_pointer()) {
        return;
    }

    uint32_t now = AP_HAL::micros();
    if (now - last_pkt1_us >= 20000) {
        last_pkt1_us = now;
        send_packet1();
    }

    if (now - last_pkt2_us >= 200000) {
        last_pkt2_us = now;
        send_packet2();
    }
}