/*-------------------------------------------------------
* LIGHT TELEMETRY FILE READER 3.3
* DUPE 20220801 20240709 20240915
* Read bytes from LTM binfile and display LTM data in CSV 
* Use: ltmread binfile 
* CO: https://github.com/KipK/Ghettostation/wiki
---------------------------------------------------------*/
#include <stdio.h>
#include <stdint.h> 
#include <stdbool.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>
#include "LightTelemetry.h"
//#define MAXCHAR 350000
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 = 6; // minimum sat for calc distance
int32_t gps_range = 10000000; //precision gps for calc distance
uint16_t bat_min = 30000;
uint16_t bat_range = 8000; // minimum vbat
uint16_t minspeed = 5; // minimum groundspeed
int32_t gps_homelat = 0; 
int32_t gps_homelon = 0;
int32_t gps_lastlat = 0; 
int32_t gps_lastlon = 0;
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;
uint8_t gps_ok = 0;
uint8_t gps_homeok = 0;
int dist_total = 0;
int dist_max = 0;
int32_t alt_max = 0;
uint16_t speed_max = 0;
int16_t pitchUp_max = 0; // -180
int16_t pitchDown_max = 0; // 180
int16_t rollLeft_max = 0;  // - 180
int16_t rollRight_max = 0; // 180
unsigned int byte;
unsigned int LTMG = 0;
unsigned int LTMA = 0;
unsigned int LTMO = 0;
unsigned int LTMN = 0;
unsigned int LTMX = 0;
unsigned int LTMS = 0;
void displayLTM();
uint16_t ftime_min = 0;
float ftime_sec = 0;
float ftime = 0;
unsigned char flightmode[7]= "N";
unsigned char gpsmode[7] = "N";
unsigned char navmode[10] ="N";
unsigned char arm[7] = "N";
unsigned char homefixstatus[7] = "N";


int main(int argc, char* argv[]) {
    FILE* file;
    unsigned long fileLen = 0;
    // unsigned char cc[MAXCHAR];
    unsigned char* cc = (char*)(malloc(fileLen * sizeof(char)));
    if (argc == 1) {
        printf("Use a file name as the argument");
        return 1;
    }
    file = fopen(argv[1], "rb");        
    if (!file) {
     printf ("Unable to open file %s", argv[1]);
     return 1;
    }
    fseek(file, 0, SEEK_END);
    fileLen = ftell(file);
    fseek(file, 0, SEEK_SET);
    fileLen = fileLen;
    //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 smaller than 300kB");
    //    return 1;
    //}
    fread(&cc, 1, fileLen, file);
    printf("FTIME;VBAT;FLIGHTMODE;GPSMODE;NAVMODE;ALT;SPEED;HEAD;ROLL;PITCH;");
    printf("LAT;LON;HLAT;HLON;HHEAD;HDIST;TDIST;");
    printf("AMP;RSSI;HDOP;SAT;ARM;HOMEFIX;FIXTYPE;GPSOK;GPSHOK;FAILSAFE;");
    printf("NAVERROR;HWSTATUS;WPNUMBER;LTMOK;LTMKO;BYTES\n");
    for (byte = 0; byte < fileLen; byte++)
    {
    // printf("%X ", cc[byte]);
 	ltm_read(cc[byte]);
    }
    printf("\n------------------------------------------\n");
    printf(" RouteDistance: %i m\n", dist_total);
    printf(" MaxDistance: %i m\n", dist_max);
    printf(" MaxAltitude: %i m\n", alt_max);
    printf(" MaxSpeed: %i km/h\n", speed_max );
    printf(" MinBattery: %.2f V\n", (float)(bat_min)/1000);
    printf(" MaxRoll-Left: %i \n", rollLeft_max);
    printf(" MaxRoll-Right: %i\n", rollRight_max);
    printf(" MaxPitch-Up: %i\n", pitchUp_max);
    printf(" MaxPitch-Down: %i\n", pitchDown_max);
    printf(" LTM-GPS: %i\n", LTMG);
    printf(" LTM-ATT: %i\n", LTMA);
    printf(" LTM-STS: %i\n", LTMS);
    printf(" LTM-ORG: %i\n", LTMO);
    printf(" LTM-NAV: %i\n", LTMN);
    printf(" LTM-EXT: %i\n", LTMX);
    printf(" LTM-OK: %i\n", LTM_pkt_ok);
    printf(" LTM-KO: %i\n", LTM_pkt_ko);
    printf(" BYTES: %i\n", byte);
    printf(" FTIME: %i:%.0f \n", ftime_min, ftime_sec);
    printf("------------------------------------------\n");
    fclose(file);
    free(cc);
	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;
        }
        if ((gps_fix) && (uav_lat > gps_minlat) && (uav_lat < gps_maxlat) && (uav_lon > gps_minlon) && (uav_lon < gps_maxlon)) {
            gps_ok = 1; 
            if (gps_lastlat == 0) { gps_lastlat = uav_lat; }
            if (gps_lastlon == 0) { gps_lastlon = uav_lon; }
        }
        else {
            gps_ok = 0;
        }
        if ((gps_ok) && (gps_homeok) && (uav_homefixstatus) && (uav_groundspeed > minspeed))
        {
            GPS_dist(&gps_lastlat, &gps_lastlon, &uav_lat, &uav_lon, &ground_distance, &ground_course);
            GPS_dist(&uav_lat, &uav_lon, &uav_homelat, &uav_homelon, &home_distance, &home_heading);
            dist_total += ground_distance;
            gps_lastlat = uav_lat;
            gps_lastlon = uav_lon;
        if (uav_groundspeed > speed_max) speed_max = uav_groundspeed;
        if (uav_alt > alt_max) alt_max = uav_alt;
        if (home_distance > dist_max) dist_max = home_distance;
        }
        LTMG++;
        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();
    if (uav_heading < 0 ) uav_heading = uav_heading + 360; //convert from -180/180 to 0/360°
    if ((uav_pitch < pitchUp_max) && (uav_pitch < 0) && (uav_pitch > -180))  pitchUp_max = uav_pitch;
    if ((uav_pitch > pitchDown_max) && (uav_pitch > 0) && (uav_pitch < 180)) pitchDown_max = uav_pitch;
    if ((uav_roll < rollLeft_max) && (uav_roll < 0) && (uav_roll > -180)) rollLeft_max = uav_roll;
    if ((uav_roll > rollRight_max) && (uav_roll > 0) && (uav_roll < 180)) rollRight_max = uav_roll;
    LTMA++; 
    displayLTM();
    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;
    if ((uav_bat < bat_min) && (uav_bat > bat_range)) bat_min = uav_bat;
    switch (uav_flightmode) {
    case 0: flightmode[0] = 'M'; flightmode[1] = 'A'; flightmode[2] = 'N'; flightmode[3] = 'U'; flightmode[4] = 'A'; flightmode[5] = 'L'; flightmode[6] = '\0';  break;
    case 1: flightmode[0] = 'R'; flightmode[1] = 'A'; flightmode[2] = 'T'; flightmode[3] = 'E'; flightmode[4] = '\0'; flightmode[5] = '\0'; flightmode[6] = '\0';  break;
    case 2: flightmode[0] = 'A'; flightmode[1] = 'N'; flightmode[2] = 'G'; flightmode[3] = 'L'; flightmode[4] = 'E'; flightmode[5] = '\0'; flightmode[6] = '\0'; break;
    case 3: flightmode[0] = 'H'; flightmode[1] = 'O'; flightmode[2] = 'R'; flightmode[3] = 'I'; flightmode[4] = 'Z'; flightmode[5] = 'N'; flightmode[6] = '\0';  break;
    case 4: flightmode[0] = 'A'; flightmode[1] = 'C'; flightmode[2] = 'R'; flightmode[3] = 'O'; flightmode[4] = '\0'; flightmode[5] = '\0'; flightmode[6] = '\0';  break;
    case 8: flightmode[0] = 'A'; flightmode[1] = 'L'; flightmode[2] = 'T'; flightmode[3] = 'H'; flightmode[4] = 'L'; flightmode[5] = 'D'; flightmode[6] = '\0';  break;
    case 9: flightmode[0] = 'G'; flightmode[1] = 'P'; flightmode[2] = 'S'; flightmode[3] = 'H'; flightmode[4] = 'L'; flightmode[5] = 'D'; flightmode[6] = '\0'; break;
    case 10: flightmode[0] = 'W'; flightmode[1] = 'A'; flightmode[2] = 'Y'; flightmode[3] = 'P'; flightmode[4] = 'N'; flightmode[5] = 'T'; flightmode[6] = '\0'; break;
    case 12: flightmode[0] = 'C'; flightmode[1] = 'I'; flightmode[2] = 'R'; flightmode[3] = 'C'; flightmode[4] = 'L'; flightmode[5] = 'E'; flightmode[6] = '\0'; break;
    case 13: flightmode[0] = 'R'; flightmode[1] = 'T'; flightmode[2] = 'H'; flightmode[3] = 'O'; flightmode[4] = 'M'; flightmode[5] = 'E'; flightmode[6] = '\0';  break;
    case 15: flightmode[0] = 'L'; flightmode[1] = 'A'; flightmode[2] = 'N'; flightmode[3] = 'D'; flightmode[4] = '\0'; flightmode[5] = '\0'; flightmode[6] = '\0';  break;
    case 18: flightmode[0] = 'C'; flightmode[1] = 'R'; flightmode[2] = 'U'; flightmode[3] = 'I'; flightmode[4] = 'S'; flightmode[5] = 'E'; flightmode[6] = '\0';  break;
    // default: flightmode[0] = '-'; flightmode[1] = '-'; flightmode[2] = '\0'; flightmode[3] = '\0'; flightmode[4] = '\0'; flightmode[5] = '\0'; flightmode[6] = '\0'; break;
    default: snprintf(flightmode, 7, "%d", uav_flightmode);
    }
    if (uav_arm) 
    { arm[0] = 'A'; arm[1] = 'R'; arm[2] = 'M'; arm[3] = 'E'; arm[4] = 'D'; arm[5] = '\0'; arm[6] = '\0';}
    else 
    { arm[0] = 'N'; arm[1] = 'O'; arm[2] = 'A'; arm[3] = 'R'; arm[4] = 'M'; arm[5] = '\0'; arm[6] = '\0'; }
    LTMS++;
    memset(LTMserialBuffer, 0, LIGHTTELEMETRY_SFRAMELENGTH - 4);
  }
  if (LTMcmd == LIGHTTELEMETRY_OFRAME)
  {
      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();
      uav_homefixstatus = (int8_t)ltmread_u8();
      if ((gps_ok) && (uav_homelat > gps_minlat) && (uav_homelat < gps_maxlat) && (uav_homelon > gps_minlon) && (uav_homelon < gps_maxlon))
      { gps_homeok = 1;}
      else
      {gps_homeok = 0;}
      if (uav_homefixstatus)
      {homefixstatus[0] = 'H'; homefixstatus[1] = 'F'; homefixstatus[2] = 'I'; homefixstatus[3] = 'X'; homefixstatus[4] = '\0'; homefixstatus[5] = '\0'; homefixstatus[6] = '\0';}
      else
      {homefixstatus[0] = 'N'; homefixstatus[1] = 'O'; homefixstatus[2] = 'F'; homefixstatus[3] = 'I'; homefixstatus[4] = 'X'; homefixstatus[5] = '\0'; homefixstatus[6] = '\0';}
    
    LTMO++;
    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();
    switch (uav_gpsmode) {
    case 0: gpsmode[0] = 'N'; gpsmode[1] = 'O'; gpsmode[2] = '\0'; gpsmode[3] = '\0'; gpsmode[4] = '\0'; gpsmode[5] = '\0'; gpsmode[6] = '\0';  break;
    case 1: gpsmode[0] = 'P'; gpsmode[1] = 'O'; gpsmode[2] = 'S'; gpsmode[3] = 'H'; gpsmode[4] = 'L'; gpsmode[5] = 'D'; gpsmode[6] = '\0';  break;
    case 2: gpsmode[0] = 'R'; gpsmode[1] = 'T'; gpsmode[2] = 'H'; gpsmode[3] = '\0'; gpsmode[4] = '\0'; gpsmode[5] = '\0'; gpsmode[6] = '\0'; break;
    case 3: gpsmode[0] = 'M'; gpsmode[1] = 'I'; gpsmode[2] = 'S'; gpsmode[3] = 'S'; gpsmode[4] = 'I'; gpsmode[5] = 'O'; gpsmode[6] = '\0';  break;
    default:  snprintf(gpsmode, 7, "%d", uav_gpsmode);
    }
    switch (uav_navmode) {
    case 0: navmode[0] = 'N'; navmode[1] = 'O'; navmode[2] = '\0'; navmode[3] = '\0'; navmode[4] = '\0'; navmode[5] = '\0'; navmode[6] = '\0';  break;
    case 1: navmode[0] = 'R'; navmode[1] = 'T'; navmode[2] = 'H'; navmode[3] = 'S'; navmode[4] = 'T'; navmode[5] = 'A'; navmode[6] = 'R'; navmode[7] = 'T'; navmode[8] = '\0'; navmode[9] = '\0';  break;
    case 2: navmode[0] = 'R'; navmode[1] = 'T'; navmode[2] = 'H'; navmode[3] = 'R'; navmode[4] = 'O'; navmode[5] = 'U'; navmode[6] = 'T'; navmode[7] = 'E'; navmode[8] = '\0'; navmode[9] = '\0';  break;
    case 3: navmode[0] = 'P'; navmode[1] = 'O'; navmode[2] = 'S'; navmode[3] = 'H'; navmode[4] = 'O'; navmode[5] = 'L'; navmode[6] = 'D'; navmode[7] = 'I'; navmode[8] = '\0'; navmode[9] = '\0';  break;
    case 4: navmode[0] = 'P'; navmode[1] = 'O'; navmode[2] = 'S'; navmode[3] = 'H'; navmode[4] = 'O'; navmode[5] = 'L'; navmode[6] = 'D'; navmode[7] = 'T'; navmode[8] = '\0'; navmode[9] = '\0';  break;
    case 5: navmode[0] = 'W'; navmode[1] = 'P'; navmode[2] = 'R'; navmode[3] = 'O'; navmode[4] = 'U'; navmode[5] = 'T'; navmode[6] = 'E'; navmode[7] = '\0'; navmode[8] = '\0'; navmode[9] = '\0';  break;
    case 13: navmode[0] = 'R'; navmode[1] = 'T'; navmode[2] = 'H'; navmode[3] = 'H'; navmode[4] = 'O'; navmode[5] = 'V'; navmode[6] = 'E'; navmode[7] = 'R'; navmode[8] = '\0'; navmode[9] = '\0';  break;
    default:  snprintf(navmode, 10, "%d", uav_navmode);
    }
    LTMN++;
    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();
    LTMX++;
    memset(LTMserialBuffer, 0, LIGHTTELEMETRY_XFRAMELENGTH - 4);
  }
}
void displayLTM() {
  ftime = (float)(LTMA / 2.5);
  ftime_min = (int)(ftime / 60);
  ftime_sec = (float)60 * (ftime / 60 - ftime_min);
  printf("%i:%.0f;%.2f;%s;%s;%s;%i;%i;%i;%i;%i", ftime_min, ftime_sec, (float)uav_bat / 1000, flightmode, gpsmode, navmode, uav_alt, uav_groundspeed, uav_heading, uav_roll, uav_pitch);
  printf(";%i;%i;%i;%i;%i;%i;%i", uav_lat, uav_lon, uav_homelat, uav_homelon, home_heading, home_distance, dist_total);
  printf(";%i;%i;%i;%i;%s;%s;%i;%i;%i;%i", uav_amp, uav_rssi, uav_HDOP, uav_satellites_visible, arm, homefixstatus, uav_fix_type, gps_ok, gps_homeok, uav_failsafe);
  printf(";%i;%i;%i;%i;%i;%i\n",ltm_naverror, uav_HWstatus, uav_WPnumber, LTM_pkt_ok, LTM_pkt_ko, byte);
}