/*-------------------------------------------------
* LIGHT TELEMETRY (LTM) LORA RECEIVER ver 4.4
* Receive LTM Data via LoRa 433MHz - Write RAW LTM Data to SD Card - Display Decoded LTM Data on TFT 
* DUPE 20220730, 20240722 (NEW TFT 480x320 ST7796s)
* CO: https:*github.com/KipK/Ghettostation/wiki 
* HW: STM32F103C (BLUEPILL) + LORA RECEIVER E32TTL100 + TFT/SD ST7796(SPI)
* UART1: RX->PA9,TX->PA10  (LoRa)
* SPI1: SDA/MOSI-PA7, CLK/SCK-PA5, CS-PA4, RST-PA0, RS/DC-PA1 (TFT)
* SPI2: MOSI-PB15, MISO-PB14, SCK-PB13, CS-PB12 (SD)
* VCC 3.3V - step down from 2S (8V) 
----------------------------------------------------*/
#ifdef membug
#include <MemoryFree.h>
#endif
#include <TFT_eSPI.h> 
#include <SPI.h>
#include <SD.h> 
#include "LightTelemetry.h"
#include <stdint.h>
#define SD_CS  PB0
#define LTM_BAUDS     9600                       
// HardwareSerial Serial1(PA10, PA9);
TFT_eSPI tft = TFT_eSPI(); 
File dataFile;
uint8_t gps_minsatfix = 4;
int32_t gps_range = 10000000;
uint16_t max_alt = 1000;
uint16_t min_speed = 5;
uint16_t max_speed = 250;
uint8_t gps_ok = 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;
int32_t route_distance = 0;
uint32_t flight_start = 0;
uint32_t flight_time = 0;
uint8_t  existFile = 1;
uint8_t numberFile = 1;
String errMessage ="Start-LTM-OK";
String prefixnameFile = "/ltm";
String nameFile = "/ltm1.dat";
static uint8_t LTMBuffer[LIGHTTELEMETRY_GFRAMELENGTH - 4];
static uint8_t LTMreceiverIndex;
static uint8_t LTMcmd;
static uint8_t LTMrcvChecksum;
static uint8_t LTMreadIndex;
static uint8_t LTMframelength;
uint32_t row = 0;
uint32_t display_clear = 0;
String flightmode = "N";
String navmode ="N";
String gpsmode ="N";
String arm = "N";
String LTM_pkt = "N";
int32_t LTM_lastpkt_ok = 0;

uint8_t ltmread_u8()  {
  return LTMBuffer[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 ltm_read() {
    uint8_t c;
    static enum _Serial1_state {
    IDLE,
    HEADER_START1,
    HEADER_START2,
    HEADER_MSGTYPE,
    HEADER_DATA
  }
  c_state = IDLE;
  while (Serial1.available()) {
    c = char(Serial1.read());  
   // Serial.printf("%02x ", c);
    if (dataFile) {
      dataFile.write(c);
      errMessage = "Writing " + nameFile;
      }
    else {errMessage = "ERROR write to file";}   
    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;
          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) { 
        if (LTMrcvChecksum == 0) {
          LTM_pkt_ok++;
          ltm_check();
          c_state = IDLE;
        }
        else {        

          LTM_pkt_ko++;       
          c_state = IDLE;         
        }
      }
      else LTMBuffer[LTMreceiverIndex++] = c;
    }
  }
}
// Decoded received commands
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
    uint8_t ltm_satsfix = ltmread_u8();
    uav_satellites_visible = (ltm_satsfix >> 2) & 0xFF;
    uav_fix_type = ltm_satsfix & 0b00000011;
    memset(LTMBuffer, 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°
    memset(LTMBuffer, 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;
    switch (uav_flightmode) {
    case 0: flightmode = "MANUAL";break;
    case 1: flightmode = "RATE";break;
    case 2: flightmode = "ANGLE";break;
    case 3: flightmode = "HORIZON";break;
    case 4: flightmode = "ACRO";break;
    case 8: flightmode = "ALTHOLD";break;
    case 9: flightmode = "GPSHOLD";break;
    case 10: flightmode = "WAYPOINT";break;
    case 12: flightmode = "CIRCLE";break;
    case 13: flightmode = "RTH";break;
    case 15: flightmode = "LAND";break;
    case 18: flightmode = "CRUISE";break;
    default: flightmode = String(uav_flightmode);
    }
    switch (uav_arm) {
    case 0: arm = "NOARM";break;
    case 1: arm = "ARMED";break;
    }
    memset(LTMBuffer, 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(); 
    memset(LTMBuffer, 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_navmode) {
    case 0: navmode = "NONE";break;
    case 1: navmode = "RTH-START";break;
    case 2: navmode = "RTH-ROUTE";break;
    case 5: navmode = "WP-ROUTE";break;
    case 13: navmode = "RTH-HOVER";break;
    default: navmode = String(uav_navmode);
    }
    switch (uav_gpsmode) {
    case 0: gpsmode = "NONE";break;
    case 1: gpsmode = "POSHOLD";break;
    case 2: gpsmode = "RTH";break;
    case 3: gpsmode = "MISSION";break;
    default: gpsmode = String(uav_gpsmode);
    }
    memset(LTMBuffer, 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(LTMBuffer, 0, LIGHTTELEMETRY_XFRAMELENGTH - 4);
  }
}
// distance and direction between two points 
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 = *lat2 - *lat1;     // difference of latitude in 1/10 000 000 degrees
  float distLon = (float)(*lon2 - *lon1) * scaleLongDown;
  *dist = sqrt(sq(distLat) + sq(distLon)) * 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 display_tft_head() {
  tft.setTextColor(TFT_WHITE, TFT_BLACK);  
  tft.setTextSize(3);    // 1 = 10 pixels, size 2 =20 pixels             
  tft.setCursor(10, 10); tft.print("ALT:");
  tft.setCursor(10, 40); tft.print("SPEED: ");
  tft.setCursor(10, 70); tft.print("HEAD:");
  tft.setCursor(10, 100); tft.print("BAT:");
  tft.setCursor(10, 130); tft.print("DIST:");
  tft.drawFastHLine(5, 160,  tft.width(), TFT_WHITE); 
  tft.setTextSize(2); 
  tft.setCursor(10, 180); tft.print("SATELIT:");
  tft.setCursor(10, 200);tft.print("FMODE:");
  tft.setCursor(10, 220);tft.print("NAVMODE:");
  tft.setCursor(10, 240);tft.print("GPSMODE:");
  tft.setCursor(10, 260); tft.print("ARM-HFIX:");
  tft.setCursor(10, 280);tft.print("ROLL:");
  tft.setCursor(10, 300);tft.print("PITCH:");
  tft.setCursor(10, 320);tft.print("RSSI:");
  tft.setCursor(10, 340);tft.print("FTIME:");
  tft.setCursor(10, 360);tft.print("LAT:");
  tft.setCursor(10, 380);tft.print("LON:");
  tft.drawFastHLine(5, 400,  tft.width(), TFT_WHITE);
  }
void display_tft() {
  tft.setTextSize(3); 
  tft.setTextColor(TFT_YELLOW, TFT_BLACK);  
  tft.setCursor(120, 10); tft.print(uav_alt);
  tft.setCursor(120, 40); tft.print(uav_groundspeed);
  tft.setCursor(120, 70); tft.print(uav_heading);
  tft.setCursor(120, 100); tft.print(uav_bat/100);
  tft.setCursor(120, 130); tft.print(home_distance); tft.print("   "); tft.print(route_distance);
  tft.setTextSize(2); 
  tft.setCursor(120, 180);tft.print(uav_satellites_visible);
  tft.setCursor(120, 200);tft.print(flightmode); if (uav_failsafe == 1) {tft.print(" - !!FS!!");}
  tft.setCursor(120, 220);tft.print(navmode);
  tft.setCursor(120, 240);tft.print(gpsmode); if (uav_gpsmode == 3) {tft.print(" - WP:");tft.print(uav_WPnumber);}
  tft.setCursor(120, 260);tft.print(arm);if (uav_homefixstatus == 1) {tft.print(" - HFIX");}
  tft.setCursor(120, 280);tft.print(uav_roll);
  tft.setCursor(120, 300);tft.print(uav_pitch);
  tft.setCursor(120, 320);tft.print(uav_rssi);
  tft.setCursor(120, 340);tft.print(flight_time);tft.print(" ");tft.print(LTM_pkt_ok);tft.print(" ");tft.print(row);;
  tft.setCursor(70, 360);tft.print(uav_lat);tft.print(" ");tft.print(uav_homelat);
  tft.setCursor(70, 380);tft.print(uav_lon);tft.print(" ");tft.print(uav_homelon);
  tft.setCursor(10, 420);tft.print(LTM_pkt);
  tft.setCursor(10, 440);tft.print(errMessage);
   }
void display_tft_clear(){
  tft.setTextSize(3); 
  tft.setTextColor(TFT_BLACK, TFT_BLACK);  
  tft.setCursor(120, 10); tft.print(uav_alt);
  tft.setCursor(120, 40); tft.print(uav_groundspeed);
  tft.setCursor(120, 70); tft.print(uav_heading);
  tft.setCursor(120, 100); tft.print(uav_bat/100);
  tft.setCursor(120, 130); tft.print(home_distance); tft.print("   "); tft.print(route_distance);
  tft.setTextSize(2); 
  tft.setCursor(120, 180);tft.print(uav_satellites_visible);
  tft.setCursor(120, 200);tft.print(flightmode); if (uav_failsafe == 1) {tft.print(" - !!FS!!");}
  tft.setCursor(120, 220);tft.print(navmode);
  tft.setCursor(120, 240);tft.print(gpsmode); if (uav_gpsmode == 3) {tft.print(" - WP:");tft.print(uav_WPnumber);}
  tft.setCursor(120, 260);tft.print(arm);if (uav_homefixstatus == 1) {tft.print(" - HFIX");}
  tft.setCursor(120, 280);tft.print(uav_roll);
  tft.setCursor(120, 300);tft.print(uav_pitch);
  tft.setCursor(120, 320);tft.print(uav_rssi);
  tft.setCursor(120, 340);tft.print(flight_time);tft.print(" ");tft.print(LTM_pkt_ok);tft.print(" ");tft.print(row);;
  tft.setCursor(70, 360);tft.print(uav_lat);tft.print(" ");tft.print(uav_homelat);
  tft.setCursor(70, 380);tft.print(uav_lon);tft.print(" ");tft.print(uav_homelon);
  tft.setCursor(10, 420);tft.print(LTM_pkt);
  tft.setCursor(10, 440);tft.print(errMessage);
}
void vypocet(){
   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) && (uav_homefixstatus) && (uav_groundspeed > min_speed))
        {
            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);
            route_distance += ground_distance;
            gps_lastlat = uav_lat;
            gps_lastlon = uav_lon;
            if (flight_start = 0) {flight_start = (int) millis()/1000;}
            flight_time = (int) millis()/1000-flight_start;
        }
   }
void display_terminal() {
  //Serial.println("LoRa Receiver");
  String dataMessage1;
  String dataMessage2;
  String dataMessage3;
  String dataMessage4;
  String dataMessage;
  Serial.println(errMessage);
  dataMessage1 = String(uav_bat/100) + ";" + flightmode + ";" + gpsmode + ";" + navmode + ";" + String(uav_alt) + ";" + String(uav_groundspeed) + ";" + String(uav_roll) + ";" + String(uav_pitch) + ";" + String(uav_heading) + ";" + String(home_distance) + ";" + String(home_heading);
  dataMessage2 = String(uav_lat) + ";" + String(uav_lon) + ";" + String(uav_homelat) + ";" + String(uav_homelon) +  ";" + + ";" + String(home_heading) + ";" + String(home_distance);
  dataMessage3 = String(uav_amp) + ";" + String(uav_rssi) + ";" + String(uav_HDOP) + ";" + String(uav_satellites_visible) + ";" + String(uav_fix_type) + ";" + String(uav_homefixstatus) + ";" + String(gps_ok) + ";" + String(uav_arm) + ";" + String(uav_failsafe);
  dataMessage3 = String(ltm_naverror) + ";" + String(uav_HWstatus) + ";" + String(uav_WPnumber) + ";" + String(LTM_pkt_ok) + ";" + String(LTM_pkt_ko);
  dataMessage = dataMessage1 + ";" + dataMessage2 + ";" + dataMessage3 + ";" + dataMessage4;
  Serial.println(dataMessage);
} 

void setup() {
  tft.init();
  tft.fillScreen(TFT_BLACK); 
  display_tft_head();
  tft.setTextColor(TFT_RED, TFT_BLACK);
  tft.setTextSize(2);tft.setCursor(10, 460); tft.print(errMessage);
  Serial1.begin(9600);
  delay(500);
  SD.begin(SD_CS); 
  delay(500);
  if(!SD.begin(SD_CS)) {
   errMessage = "Card Mount Failed!";
    tft.setCursor(10, 460);tft.print(errMessage);
   return;
  }
  if (!SD.begin(SD_CS)) {
   errMessage ="Card Init Failed!";
    tft.setCursor(10, 460);tft.print(errMessage);
   return; 
   }
  while (existFile) {
   nameFile = prefixnameFile + String(numberFile) + ".dat"; 
   if (SD.exists(nameFile.c_str())) {
    numberFile++;
    } else {
     existFile = 0;    
    }
  }  
  dataFile = SD.open(nameFile.c_str(), FILE_WRITE);
  if (dataFile) {
  //dataFile.println(dataHeader);
    dataFile.seek(dataFile.size());
    errMessage = "Created " + nameFile ;
   } else {
    errMessage = "Created File Failed!";
    tft.setCursor(10, 460);tft.print(errMessage);
  }
 }

void loop() {
  row++; 
  LTM_lastpkt_ok=LTM_pkt_ok;
  ltm_read();
  if ((LTM_pkt_ok - LTM_lastpkt_ok) > 0 ){LTM_pkt="LTM-RECEIVE-OK";vypocet();}
  else {LTM_pkt="!! LTM-NORECEIVE !!";}
  // display_terminal();
  if (uav_alt < max_alt && uav_groundspeed < max_speed) {display_tft();}
  /*
  if (dataFile) {
   dataFile.println(dataMessage);
   dataFile.flush();
   errMessage = "Writing " + nameFile;
  }
  */
  dataFile.flush();
  delay(500);
  display_tft_clear();
 }