/*
 * Copyright (C) EdgeTX
 *
 * License GPLv2: http://www.gnu.org/licenses/gpl-2.0.html
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * 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.
 *
 * This file is based on code from Betaflight project
 * https://github.com/betaflight/betaflight
 *
 * Cleanflight and Betaflight are free software. You can redistribute
 * this software and/or modify this software 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.
 *
 * Cleanflight and Betaflight are distributed in the hope that they
 * 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 software.
 *
 * If not, see <http://www.gnu.org/licenses/>.
 */

#include <ctype.h>

#include "gps.h"
#include "edgetx.h"

extern const etx_serial_driver_t* gpsSerialDrv;
extern void* gpsSerialCtx;

// https://docs.ros.org/en/noetic/api/ublox_msgs/html/msg/NavPVT.html
typedef struct ubxNavPvt_s {
  uint32_t time;         // GPS Millisecond time of week [ms]
  uint16_t year;         // Year (UTC)
  uint8_t month;         // Month, range 1..12 (UTC)
  uint8_t day;           // Day of month, range 1..31 (UTC)
  uint8_t hour;          // Hour of day, range 0..23 (UTC)
  uint8_t min;           // Minute of hour, range 0..59 (UTC)
  uint8_t sec;           // Seconds of minute, range 0..60 (UTC)
  uint8_t valid;         // Validity flags
  uint32_t tAcc;         // time accuracy estimate [ns] (UTC)
  int32_t nano;          // fraction of a second [ns], range -1e9 .. 1e9 (UTC)
  uint8_t fixType;       // GNSS fix Type
  uint8_t flags;         // Fix Status Flags
  uint8_t flags2;        // Additional Flags
  uint8_t numSV;         // Number of SVs used in Nav Solution
  int32_t lon;           // Longitude [deg / 1e-7]
  int32_t lat;           // Latitude [deg / 1e-7]
  int32_t height;        // Height above Ellipsoid [mm]
  int32_t hMSL;          // Height above mean sea level [mm]
  uint32_t hAcc;         // Horizontal Accuracy Estimate [mm]
  uint32_t vAcc;         // Vertical Accuracy Estimate [mm]
  int32_t velN;          // NED north velocity [mm/s]
  int32_t velE;          // NED east velocity [mm/s]
  int32_t velD;          // NED down velocity [mm/s]
  int32_t gSpeed;        // Ground Speed (2-D) [mm/s]
  int32_t headMot;       // Heading of motion 2-D [deg / 1e-5]
  uint32_t sAcc;         // Speed Accuracy Estimate [mm/s]
  uint32_t headAcc;      // Heading Accuracy Estimate (both motion & vehicle)
  uint16_t pDOP;         // Position DOP [1 / 0.01]
  uint8_t flags3;        // Additional Flags
  uint8_t reserved0[5];  // Reserved
  int32_t headVeh;       // Heading of vehicle (2-D) [deg / 1e-5]
  int16_t magDec;        // # Magnetic declination [deg / 1e-2]
  uint16_t magAcc;       // Magnetic declination accuracy [deg / 1e-2]
} ubxNavPvt_t;

typedef struct ubxNavDop_s {
  uint32_t itow;  // GPS Millisecond Time of Week
  uint16_t gdop;  // Geometric DOP
  uint16_t pdop;  // Position DOP
  uint16_t tdop;  // Time DOP
  uint16_t vdop;  // Vertical DOP
  uint16_t hdop;  // Horizontal DOP
  uint16_t ndop;  // Northing DOP
  uint16_t edop;  // Easting DOP
} ubxNavDop_t;

typedef struct ubxCfgMsg_s {
  uint8_t msgClass;
  uint8_t msgID;
  uint8_t rate;
} ubxCfgMsg_t;

static void gpsSendMessage(uint16_t msg_type, uint16_t msg_len,
                           uint8_t* payload)
{
  if (!gpsSerialDrv) return;

  auto _sendByte = gpsSerialDrv->sendByte;
  if (!_sendByte) return;

  uint8_t ck_a, ck_b;
  gpsSerialDrv->sendByte(gpsSerialCtx, 0xb5);
  gpsSerialDrv->sendByte(gpsSerialCtx, 0x62);

  ck_b = ck_a = (msg_type >> 8);
  gpsSerialDrv->sendByte(gpsSerialCtx, msg_type >> 8);

  ck_b += (ck_a += (msg_type & 0xff));
  gpsSerialDrv->sendByte(gpsSerialCtx, msg_type & 0xff);
  ck_b += (ck_a += (msg_len & 0xff));
  gpsSerialDrv->sendByte(gpsSerialCtx, msg_len & 0xff);
  ck_b += (ck_a += (msg_len >> 8));
  gpsSerialDrv->sendByte(gpsSerialCtx, msg_len >> 8);
  for (int n = 0; n < msg_len; n++) {
    ck_b += (ck_a += payload[n]);
  }
  if (msg_len > 0) {
    gpsSerialDrv->sendBuffer(gpsSerialCtx, payload, msg_len);
  }
  gpsSerialDrv->sendByte(gpsSerialCtx, ck_a);
  gpsSerialDrv->sendByte(gpsSerialCtx, ck_b);
}

static void configureGps(bool detect)
{
  static int state = 0;

  if (detect) state = 0;

  auto txCompleted = gpsSerialDrv->txCompleted;
  if (txCompleted && !txCompleted(gpsSerialCtx)) return;

  switch (state) {
    case 0:
      gpsSendMessage(0x0a04, 0, nullptr);
      ubxCfgMsg_t val;
      val.msgClass = 0x01;
      val.msgID = 0x07;
      val.rate = 1;
      gpsSendMessage(0x0601, sizeof(ubxCfgMsg_t), (uint8_t*)&val);
      val.msgID = 0x04;
      gpsSendMessage(0x0601, sizeof(ubxCfgMsg_t), (uint8_t*)&val);
      state++;
  }
}

static void gpsProcessMessage(uint16_t msg_type, uint16_t msg_len,
                              uint8_t* payload)
{
  if (msg_type == 0x0107) {
    auto pvt = (ubxNavPvt_t*)payload;
    gpsData.fix = pvt->flags & 0x01;
    gpsData.numSat = pvt->numSV;
    gpsData.speed = pvt->gSpeed / 100;            // speed in 0.1m/s
    gpsData.groundCourse = pvt->headMot / 10000;  // degrees * 10
    if (gpsData.fix) {
      gpsData.longitude = pvt->lon / 10;   // degrees * 1.000.000
      gpsData.latitude = pvt->lat / 10;    // degrees * 1.000.000
      gpsData.altitude = pvt->hMSL / 100;  // altitude in 0.1m
    }

#if defined(RTCLOCK)
    // set RTC clock if needed
    if (g_eeGeneral.adjustRTC && (pvt->valid & 0x03) == 0x03) {
      rtcAdjust(pvt->year, pvt->month, pvt->day, pvt->hour, pvt->min, pvt->sec);
    }
#endif
  }

  if (msg_type == 0x0104) {
    auto dop = (ubxNavDop_t*)payload;
    gpsData.hdop = dop->hdop;
  }
}

bool gpsNewFrameUBX(uint8_t c, bool detect)
{
  configureGps(detect);

  const auto BUFFER_SIZE = 100u;
  static int state = 0;
  static uint16_t msg_type;
  static uint16_t msg_len;
  static uint16_t msg_pos;
  static uint8_t buf[BUFFER_SIZE];
  static uint8_t ck_a;
  static uint8_t ck_b;

  bool result = false;

  switch (state) {
    case 0:  // Search Sync Char 1
      if (c == 0xb5) {
        state = 1;
      }
      break;
    case 1:  // Search Sync Char 2
      if (c == 0x62) {
        state = 2;
      } else {
        state = 0;
      }
      break;
    case 2:  // Get message class
      ck_b = ck_a = c;
      msg_type = c << 8;
      state = 3;
      break;
    case 3:  // Get message id
      ck_b += (ck_a += c);
      msg_type |= c;
      state = 4;
      break;
    case 4:  // Get length 1
      ck_b += (ck_a += c);
      msg_len = c;
      state = 5;
      break;
    case 5:  // Get length 2
      ck_b += (ck_a += c);
      msg_len |= c << 8;
      msg_pos = 0;
      if (msg_len == 0) {
        state = 7;
        break;
      }
      if ((msg_type != 0x0107 && msg_type != 0x0104) || msg_len > BUFFER_SIZE) {
        state = 9;
        break;
      }
      state = 6;
      break;
    case 6:  // Get payload
      ck_b += (ck_a += c);
      buf[msg_pos++] = c;
      if (msg_pos >= msg_len) {
        state = 7;
      }
      break;
    case 7:  // Get checksum 1
      if (c == ck_a) {
        state = 8;
      } else {
        gpsData.errorCount++;
        state = 0;
      }
      break;
    case 8:  // Get checksum 2, end of frame
      if (c == ck_b) {
        gpsProcessMessage(msg_type, msg_len, buf);
        gpsData.packetCount++;
        result = true;
      } else {
        gpsData.errorCount++;
      }
      state = 0;
      break;
    case 9:  // Discard message
      ck_b += (ck_a += c);
      msg_pos++;
      if (msg_pos >= msg_len) {
        state = 10;
        result = true;
      }
      break;
    case 10:  // Get checksum 1 of discarded message
      if (c == ck_a) {
        state = 11;
      } else {
        gpsData.errorCount++;
        state = 0;
      }
      break;
    case 11:  // Get checksum 2 of discarded message
      if (c == ck_b) {
        // We have a valid UBX message
        gpsData.packetCount++;
        result = true;
      } else {
        gpsData.errorCount++;
      }
      state = 0;
      break;
  }

  return result;
}