/*-------------------------------------------------------
* LIGHT TELEMETRY GPS READER 2.2
* DUPE 20220730
* Read bytes from LTM binary file and display GPS data in GPX format
* Use: ltmread binfile <bytes>
* CO: https://github.com/KipK/Ghettostation/wiki
* TODO: dynamic alocc memory
---------------------------------------------------------*/
#include <stdio.h>
#include <stdint.h> 
#include <stdbool.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include "LightTelemetry.h"
#define MAXCHAR 150000
static uint8_t LTMserialBuffer[LIGHTTELEMETRY_GFRAMELENGTH-4];
static uint8_t LTMreceiverIndex;
static uint8_t LTMcmd;
static uint8_t LTMrcvChecksum;
static uint8_t LTMreadIndex;
static uint8_t LTMframelength; 
uint8_t gps_minsatfix = 4;
int32_t gps_range = 10000000;
int32_t gps_minlon = 0;
int32_t gps_maxlon = 0;
int32_t gps_minlat = 0;
int32_t gps_maxlat = 0;
uint8_t gps_fix = 0;
unsigned int byte;
unsigned int LTMA = 0;
//float ftime;
uint16_t ftime_min = 0;
float ftime_sec = 0;

int main(int argc, char* argv[])
{
    FILE* file;
    unsigned long fileLen, fileLen1;
    unsigned char cc[MAXCHAR];

    file = fopen(argv[1], "rb");
    if (!file) {
        printf ("Unable to open file %s", argv[1]);
        return 1;
    }
    fseek(file, 0, SEEK_END);
    fileLen1 = ftell(file);
    fseek(file, 0, SEEK_SET);
    fileLen = fileLen1;
    if (argc == 3)  fileLen = atoi(argv[2]); 
    //printf("File bytes:%i ", fileLen1);
    //printf("Read bytes:%i\r\n", fileLen);
    if (fileLen > MAXCHAR) {
        printf ("Use file or argument smaller than 150kB");
        return 1;
    }
    fread(&cc, 1, fileLen, file);
    printf("<?xml version=\"1.0\" encoding=\"UTF-8\"?>\n");
    printf("<gpx creator=\"LTM\" version=\"1.1\" xmlns=\"http://www.topografix.com/GPX/1/1\" xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\" xsi:schemaLocation=\"http://www.topografix.com/GPX/1/1 http://www.topografix.com/GPX/1/1/gpx.xsd\">\n");
    printf("<metadata><name>LTM</name></metadata>\n");
    printf("<trk><name>LTM</name><trkseg>\n");
    for (byte = 0; byte < fileLen; byte++) {
    // printf("%X ", cc[byte]);
 	ltm_read(cc[byte]);
    } 
    printf("</trkseg></trk>\n");
    printf("</gpx>\n");
	fclose(file);
	return 0;
}
uint8_t ltmread_u8()  {
  return LTMserialBuffer[LTMreadIndex++];
}
uint16_t ltmread_u16() {
  uint16_t t = ltmread_u8();
  t |= (uint16_t)ltmread_u8()<<8;
  return t;
}
uint32_t ltmread_u32() {
  uint32_t t = ltmread_u16();
  t |= (uint32_t)ltmread_u16()<<16;
  return t;
}
void GPS_dist(int32_t* lat1, int32_t* lon1, int32_t* lat2, int32_t* lon2, int32_t* dist, int32_t* bearing)
{
    float rads = (abs((float)*lat2) / 10000000.0) * 0.0174532925; // latitude to radians
    double scaleLongDown = cos(rads); // calculate longitude scaling 
    float distLat = (float)(*lat2 - *lat1); // difference of latitude in 1/10 000 000 degrees
    double distLon = (double)(*lon2 - *lon1) * scaleLongDown;
    *dist = (int)(sqrt(pow(distLat, 2) + pow(distLon, 2)) * 1.113195 / 100); // distance between two GPS points in m
    *bearing = (int)round(90 + atan2(-distLat, distLon) * 57.2957795); //azimut convert the output radians to deg
    if (*bearing < 0) *bearing += 360;
}

void ltm_read(uint8_t c) {
 // printf("BYTE:%hhX ", c);
   static enum _serial_state {
    IDLE,
    HEADER_START1,
    HEADER_START2,
    HEADER_MSGTYPE,
    HEADER_DATA
  }
  c_state = IDLE;

      if (c_state == IDLE) {
      c_state = (c == '$') ? HEADER_START1 : IDLE;
      }
    else if (c_state == HEADER_START1) {
      c_state = (c == 'T') ? HEADER_START2 : IDLE;
      }
    else if (c_state == HEADER_START2) {
      switch (c) {
        case 'G':
          LTMframelength = LIGHTTELEMETRY_GFRAMELENGTH;
          c_state = HEADER_MSGTYPE;
          break;
        case 'A':
          LTMframelength = LIGHTTELEMETRY_AFRAMELENGTH;
          c_state = HEADER_MSGTYPE;
          break;
        case 'S':
          LTMframelength = LIGHTTELEMETRY_SFRAMELENGTH;
          c_state = HEADER_MSGTYPE;
          break;
        case 'O':
          LTMframelength = LIGHTTELEMETRY_OFRAMELENGTH;
          c_state = HEADER_MSGTYPE;
          break;
        case 'N':
          LTMframelength = LIGHTTELEMETRY_NFRAMELENGTH;
          c_state = HEADER_MSGTYPE;
          break;
        case 'X':
          LTMframelength = LIGHTTELEMETRY_XFRAMELENGTH;
          c_state = HEADER_MSGTYPE;
          // printf("HEAD-$\ ");
          break;
        default:
          c_state = IDLE;
      }
      LTMcmd = c;
      LTMreceiverIndex = 0;
    }
    else if (c_state == HEADER_MSGTYPE) {
      if (LTMreceiverIndex == 0) {
        LTMrcvChecksum = c;
      }
      else {
        LTMrcvChecksum ^= c;
      }
       if (LTMreceiverIndex == LTMframelength - 4) 
        {
            // printf(" CheckSum:%hhX\r\n", LTMrcvChecksum);
            if (LTMrcvChecksum == 0) {
            LTM_pkt_ok++;   
            ltm_check();
            c_state = IDLE;
        }
        else {                                                  
            LTM_pkt_ko++;       
            c_state = IDLE;
             }
      }
      else LTMserialBuffer[LTMreceiverIndex++] = c;
    }
}
void ltm_check() 
{
  LTMreadIndex = 0;
  if (LTMcmd == LIGHTTELEMETRY_GFRAME)
  {
    uav_lat = (int32_t)ltmread_u32();
    uav_lon = (int32_t)ltmread_u32();
    uav_groundspeedms = ltmread_u8();
    uav_groundspeed = (uint16_t) round((float)(uav_groundspeedms * 3.6f)); // convert to kmh
    uav_alt = (int32_t)ltmread_u32()/100;//convert to m
    ltm_satsfix = ltmread_u8();
    uav_satellites_visible = (ltm_satsfix >> 2) & 0xFF;
    uav_fix_type = ltm_satsfix & 0b00000011;
    
    if ((uav_satellites_visible >= gps_minsatfix) && (gps_fix == 0)) {
        gps_maxlat = uav_lat + gps_range;
        gps_minlat = uav_lat - gps_range;
        gps_maxlon = uav_lon + gps_range;
        gps_minlon = uav_lon - gps_range;
        gps_fix = 1;
    }
    float ftime = (float)(LTMA / 5);
    ftime_min = (int)(ftime/60);
    ftime_sec = (float)60*(ftime/60-ftime_min);
    if ((uav_satellites_visible >= gps_minsatfix) && (uav_lat > gps_minlat) && (uav_lat < gps_maxlat) && (uav_lon > gps_minlon) && (uav_lon < gps_maxlon)) {
        // printf("FTIME : % f ", ftime);
        if ((ftime_min < 10) && (ftime_sec < 10))
        printf("<trkpt lat=\"%.7f\" lon=\"%.7f\"><ele>%i</ele><time>2000-01-01T00:0%i:0%.1fZ</time></trkpt>\n", (double)uav_lat / 10000000, (double)uav_lon / 10000000, uav_alt, ftime_min, ftime_sec);
        else if ((ftime_min >= 10) && (ftime_sec >= 10))
        printf("<trkpt lat=\"%.7f\" lon=\"%.7f\"><ele>%i</ele><time>2000-01-01T00:%i:%.1fZ</time></trkpt>\n", (double)uav_lat / 10000000, (double)uav_lon / 10000000, uav_alt, ftime_min, ftime_sec);       
        else if ((ftime_min >= 10) && (ftime_sec < 10))
        printf("<trkpt lat=\"%.7f\" lon=\"%.7f\"><ele>%i</ele><time>2000-01-01T00:%i:0%.1fZ</time></trkpt>\n", (double)uav_lat / 10000000, (double)uav_lon / 10000000, uav_alt, ftime_min, ftime_sec);
        else if ((ftime_min < 10) && (ftime_sec >= 10))
        printf("<trkpt lat=\"%.7f\" lon=\"%.7f\"><ele>%i</ele><time>2000-01-01T00:0%i:%.1fZ</time></trkpt>\n", (double)uav_lat / 10000000, (double)uav_lon / 10000000, uav_alt, ftime_min, ftime_sec);

        }
    memset(LTMserialBuffer, 0, LIGHTTELEMETRY_GFRAMELENGTH - 4);
  }
  if (LTMcmd == LIGHTTELEMETRY_AFRAME)
  {
    uav_pitch = (int16_t)ltmread_u16();
    uav_roll =  (int16_t)ltmread_u16();
    uav_heading = (int16_t)ltmread_u16();
    LTMA++;
    memset(LTMserialBuffer, 0, LIGHTTELEMETRY_AFRAMELENGTH - 4);
  }
  if (LTMcmd == LIGHTTELEMETRY_SFRAME)
  {
    uav_bat = ltmread_u16();
    uav_amp = ltmread_u16();
    uav_rssi = (ltmread_u8()*100)/255;
    uav_airspeed = ltmread_u8()*100;
    ltm_armfsmode = ltmread_u8();
    uav_arm = ltm_armfsmode & 0b00000001;
    uav_failsafe = (ltm_armfsmode >> 1) & 0b00000001;
    uav_flightmode = (ltm_armfsmode >> 2) & 0b00111111;
    memset(LTMserialBuffer, 0, LIGHTTELEMETRY_SFRAMELENGTH - 4);
  }
  if (LTMcmd == LIGHTTELEMETRY_OFRAME) // origin frame
  {
    uav_homelat = (int32_t)ltmread_u32();
    uav_homelon = (int32_t)ltmread_u32();
    uav_homealt = (int32_t)ltmread_u32();
    uav_osd_on = (int8_t)ltmread_u8(); // always 1
    uav_homefixstatus = (int8_t)ltmread_u8(); // home fix status
    memset(LTMserialBuffer, 0, LIGHTTELEMETRY_OFRAMELENGTH - 4);
  }
  if (LTMcmd == LIGHTTELEMETRY_NFRAME)
  {
    uav_gpsmode = ltmread_u8();
    uav_navmode = ltmread_u8();
    uav_navaction = ltmread_u8();
    uav_WPnumber = ltmread_u8();
    ltm_naverror = ltmread_u8();
    ltm_flags = ltmread_u8();
    memset(LTMserialBuffer, 0, LIGHTTELEMETRY_NFRAMELENGTH - 4);
  }
  if (LTMcmd == LIGHTTELEMETRY_XFRAME)
  {
    uav_HDOP = ltmread_u16();
    uav_HWstatus = ltmread_u8();
    uav_spare1 = ltmread_u8();
    uav_spare2 = ltmread_u8();
    ltm_spare3 = ltmread_u8();
    memset(LTMserialBuffer, 0, LIGHTTELEMETRY_XFRAMELENGTH - 4);
  }
}