/*-------------------------------------------------------
* LIGHT TELEMETRY FILE READER 2.2
* DUPE 20220801
* Read bytes from LTM binfile and display LTM data in CSV 
* Use: ltmread binfile <bytes> (deafult/max 150000 B)
* 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; // 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
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_home_fix = 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;

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

    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);
    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("FTIME;VBAT,ALT;SPEED,DIST;HDIST;HEAD;HHEAD;ROLL;PITCH;LAT;LON;HLAT;HLON\n");
    // printf("SAT;FIXTYPE,GPSOK;LTMOK;LTMKO;BYTE");
    // printf("AMP; RSSI; ARMFS; ARM; FAILSAFE; FLIGHTMODE; HFIX; GPSMODE; NAVMODE; NAVACT; WP; NAVERR; FLAGS; HDOP; HWSTATUS");
    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: %.2f min\n", (float)(LTMA/5)/60);
    printf("------------------------------------------\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;
        }
        if ((uav_satellites_visible >= gps_minsatfix) &&(uav_lat > gps_minlat) && (uav_lat < gps_maxlat) && (uav_lon > gps_minlon) && (uav_lon < gps_maxlon)) {
            gps_ok = 1;
        }
        else {
            gps_ok = 0;
        }
        if ((gps_ok) && (uav_groundspeed > 5) && (gps_home_fix == 0))
        {
            gps_homelat = uav_lat;
            gps_homelon = uav_lon;
            gps_lastlat = uav_lat;
            gps_lastlon = uav_lon;
            gps_home_fix = 1;
        }
        if ((gps_ok) && (gps_home_fix))
        {
            GPS_dist(&gps_lastlat, &gps_lastlon, &uav_lat, &uav_lon, &ground_distance, &ground_course);
            GPS_dist(&uav_lat, &uav_lon, &gps_homelat, &gps_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;
    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(); 
    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();
    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 / 5);
  ftime_min = (int)(ftime / 60);
  ftime_sec = (float)60 * (ftime / 60 - ftime_min);
   //FTIME;VBAT,ALT;SPEED,DIST;HDIST;HEAD;HHEAD;ROLL;PITCH;LAT;LON;HLAT;HLON
   //SAT;FIXTYPE,GPSOK;LTMOK;LTMKO;BYTE
   //AMP; RSSI; ARMFS; ARM; FAILSAFE; FLIGHTMODE; HFIX; GPSMODE; NAVMODE; NAVACT; WP; NAVERR; FLAGS; HDOP; HWSTATUS
   printf("%i:%.0f;%.2f;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i\n", ftime_min, ftime_sec, (float)uav_bat/1000, uav_alt, uav_groundspeed, dist_total, home_distance, uav_heading, home_heading, uav_roll, uav_pitch, uav_lat, uav_lon, gps_homelat, gps_homelon);
   //printf(";%i;%i;%i;%i;%i;%i\n", uav_satellites_visible, uav_fix_type, gps_ok, LTM_pkt_ok, LTM_pkt_ko, byte);
   //printf(";%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i;%i\n", uav_amp, uav_rssi, ltm_armfsmode, uav_arm, uav_failsafe,uav_flightmode, uav_homefixstatus,uav_gpsmode, uav_navmode, uav_navaction, uav_WPnumber, ltm_naverror, ltm_flags, uav_HDOP, uav_HWstatus);
}