/*-------------------------------------------------
* LIGHT TELEMETRY (LTM) LORA RECEIVER ver 2.0
* DUPE20220730
* Receive LTM Stream (iNAV/Betafligt) on Serial (via LoRa 433MHz)
* Write RAW LTM Data (.dat) to SD Card
* Display Decoded LTM Data on TFT 
* -------------------------------------------------  
* HW: STM32F103C (BLUEPILL) + TFT/SD ST7735S(SPI) + LORA RECEIVER E32TTL100 
* UART1: RX->PA9   TX->PA10 
* LORA UART2: TXD->PA3   RXD->PA2
* TFT SPI1: SDA/MOSI-PA7 CLK/SCK-PA5 CS-PA4 RST-PA0 RS/DC-PA1
* SD SPI2: MOSI-PB15 MISO-PB14 SCK-PB13 CS-PB12
* CO:  https:*github.com/KipK/Ghettostation/wiki
* TODO: upgrade rewrite displayed data on TFT
* TODO: add option to switch BYTERAW|TXTCSV record file format
----------------------------------------------------*/
#ifdef membug
#include <MemoryFree.h>
#endif
#include <Adafruit_GFX.h>      // include Adafruit graphics library
#include <Adafruit_ST7735.h>   // include Adafruit ST7735 TFT library
#include <SPI.h>
#include <SD.h> 
#include "LightTelemetry.h"
#include <stdint.h>
#define TFT_RST   PA0      
#define TFT_DC    PA1
#define TFT_CS    PA4  
#define SD_CS  PB0
#define LTM_BAUDS     9600                       
// HardwareSerial Serial1(PA10, PA9);
Adafruit_ST7735 tft = Adafruit_ST7735(TFT_CS, TFT_DC, TFT_RST);
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_homereset = 0;
uint8_t gps_homefix = 0;
uint8_t gps_ok = 0;
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;
// int32_t route_distance = 0;
uint32_t flight_start = 0;
uint32_t flight_time = 0;
uint8_t  existFile = 1;
uint8_t numberFile = 1;
uint8_t recByte = 0;
String errMessage ="Starting";
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;

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());  
    if (dataFile) {
      dataFile.write(c);
      errMessage = "Writing " + nameFile;
      }
    else {errMessage = "ERR in 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;
    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(); // home fix status 
    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();
    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_bearing(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(ST7735_WHITE, ST7735_BLACK);  
  tft.setTextSize(2);    // 1 = 10 pixels, size 2 =20 pixels             
  tft.setCursor(5, 10); tft.print("ALT:");
  tft.setCursor(5, 30); tft.print("SPD: ");
  tft.setCursor(5, 50); tft.print("DST:");
  tft.setCursor(5, 70); tft.print("HED:");
  tft.setCursor(5, 90); tft.print("BAT:");
  tft.drawFastHLine(5, 110,  tft.width(), ST7735_WHITE); 
  tft.setTextSize(1);
  tft.setCursor(5, 115); tft.print("R:"); 
  tft.setCursor(5, 125);tft.print("P:"); 
  tft.setCursor(45, 115); tft.print("OK:"); 
  tft.setCursor(45, 125); tft.print("FT:");
}
void display_tft() {
  tft.setTextSize(2); 
  tft.setTextColor(ST7735_YELLOW, ST7735_BLACK);  
  tft.setCursor(60, 10); tft.print(uav_alt);
  tft.setCursor(60, 30); tft.print(uav_groundspeed);
  tft.setCursor(60, 50); tft.print(home_distance);
  tft.setCursor(60, 70); tft.print(uav_heading);
  tft.setCursor(60, 90); tft.print(uav_bat/100);
  tft.setTextSize(1); 
  tft.setTextColor(ST7735_GREEN, ST7735_BLACK);
  tft.setCursor(20, 115); tft.print(uav_roll); 
  tft.setCursor(20, 125); tft.print(uav_pitch);
  tft.setCursor(65, 115); tft.print(LTM_pkt_ok);
  tft.setCursor(65, 125); tft.print(flight_time);
  tft.setCursor(5, 135); tft.print(uav_lat); tft.setCursor(65, 135); tft.print(uav_lon);
  tft.setTextColor(ST7735_RED, ST7735_BLACK);
  tft.setCursor(5, 150); tft.print(errMessage);
  tft.setTextColor(ST7735_YELLOW, ST7735_BLACK);
}

void display_tft_clear(){
  tft.setTextSize(2); 
  tft.setTextColor(ST7735_BLACK, ST7735_BLACK);  
  tft.setCursor(60, 10); tft.print(uav_alt);
  tft.setCursor(60, 30); tft.print(uav_groundspeed);
  tft.setCursor(60, 50); tft.print(home_distance);
  tft.setCursor(60, 70); tft.print(uav_heading);
  tft.setCursor(60, 90); tft.print(uav_bat/100);
  tft.setTextSize(1); 
  tft.setTextColor(ST7735_BLACK, ST7735_BLACK); 
  tft.setCursor(20, 115); tft.print(uav_roll); 
  tft.setCursor(20, 125); tft.print(uav_pitch);
  tft.setCursor(65, 115); tft.print(LTM_pkt_ok);
  tft.setCursor(65, 125); tft.print(flight_time);
  tft.setCursor(5, 135); tft.print(uav_lat); tft.setCursor(65, 135); tft.print(uav_lon);
  tft.setCursor(5, 150); tft.print(errMessage);
}
void vypocet(){
  // String dataMessage1;
  // String dataMessage2;
  // String dataMessage3;
  // String dataMessage;
  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 > min_speed && gps_homefix == 0) {
      gps_homelat = uav_lat;
      gps_homelon = uav_lon;
      //gps_lastlat = uav_lat;
      //gps_lastlon = uav_lon;
      gps_homefix = 1;
      flight_start = (int) millis()/1000;
  } 
  if (uav_groundspeed < min_speed && gps_homefix == 1 &&  gps_homereset == 0) {
      gps_homereset = row+20;
  }
  if (uav_groundspeed < min_speed && row == gps_homereset) {  
      gps_homereset = 0;
      gps_homefix = 0;  
  }
  if (gps_homefix && uav_groundspeed > min_speed) {
      GPS_dist_bearing(&uav_lat, &uav_lon, &gps_homelat, &gps_homelon, &home_distance, &home_heading); 
      // GPS_dist(&uav_lastlat, &uav_lastlon, &uav_lat, &uav_lon, &ground_distance, &ground_course);
      //route_distance += ground_distance;
      //gps_lastlat = uav_lat;
      //gps_lastlon = uav_lon;
  }
 /* 
  dataMessage1 = String(flight_time) + ";" + String(uav_alt) + ";" + String(uav_groundspeed) + ";" + String(uav_roll) + ";" + String(uav_pitch) + ";" + String(uav_heading) + ";" + String(home_distance) + ";" + String(home_heading) + ";" + String(route_distance);
  dataMessage2 = String(uav_lat) + ";" + String(uav_lon) + ";" + String(gps_homelat) + ";" + String(gps_homelon) +  ";" + String(uav_bat) + ";" + String(uav_amp) + ";" + String(uav_satellites_visible) + ";" + String(uav_HDOP) + ";" + String(uav_rssi);
  dataMessage3 = String(uav_flightmode) + ";" + String(uav_arm) + ";" + String(uav_failsafe) + ";" + String(gps_homefix) + ";" + String(LTM_pkt_ok) + ";" + String(LTM_pkt_ko) + ";" + String(row) + ";" + String((int)millis()/1000);
  dataMessage = dataMessage1 + ";" + dataMessage2 + ";" + dataMessage3;
*/
}

void setup() {
 tft.initR(INITR_BLACKTAB);     // initialize a ST7735S chip, black tab
 tft.fillScreen(ST7735_BLACK); 
 display_tft_head();
 tft.setTextColor(ST7735_RED, ST7735_BLACK);
 tft.setCursor(5, 150); tft.print(errMessage);
 Serial1.begin(9600);
 SD.begin(SD_CS); 
  if(!SD.begin(SD_CS)) {
   errMessage = "Card Mount Failed";
   tft.print(errMessage);
   return;
  }
  if (!SD.begin(SD_CS)) {
   errMessage ="Card Init Failed !";
   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.setTextSize(1); 
  tft.setTextColor(ST7735_RED, ST7735_BLACK);
  tft.setCursor(5, 150); tft.print(errMessage);
}

void loop() {
  row++; 
  flight_time = (int) millis()/1000;
  ltm_read();
  vypocet();
  if (uav_alt < max_alt && uav_groundspeed < max_speed) {
  display_tft();
  }
  /*
  if (dataFile) {
   dataFile.println(dataMessage);
   dataFile.flush();
   errMessage = "Writing " + nameFile;
  }
  Serial.printf("%02x ", recByte);
  */
  dataFile.flush();
  delay(500);
  display_tft_clear();
 }