// Copyright (C) 2024, Michael Faragher
// Extended from the RNode Firmware. Copyright (C) 2023, Mark Qvist
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.

//#include <Boards.h>


char GPS_Buffer[100];
byte GPS_Index;
char GPSGGA[100] = " ";
char GPSGLL[100] = " ";
char GPSVTG[100] = " ";
char GPSRMC[100] = " ";
char GPSTime[6] = {'9','0','0','0','0','0'};
bool isGPS_Automated = false;
bool isGPS_Waiting_Time = false;
bool isGPS_Waiting_Location = false;
bool isGPS_Waiting_NMEA = false;
char GPS_Lat[8] = {'N','O',' ',' ','F','I','X','X'};
char GPS_Lon[9] = {'N','O',' ',' ',' ','F','I','X','X'};

#define GPS_CMD_TIME                0x00
#define GPS_CMD_LOCATION            0x01
#define GPS_CMD_RATE                0x02
#define GPS_CMD_SENTENCE            0x03
#define GPS_CMD_ENABLE_HEARTBEAT    0x10
#define GPS_CMD_DISABLE_HEARTBEAT   0x11
#define GPS_CMD_ENABLE_AUTOMATIC    0x12
#define GPS_CMD_DISABLE_AUTOMATIC   0x13
#define GPS_CMD_HOT                 0x20
#define GPS_CMD_COLD                0x21


byte NMEA_checksum(char message[], byte length){
  byte buffer = 0;
  byte working;
  for(byte i = 0; i < length; i++){
    working = message[int(i)];// Apparently not & 127; //Lower seven bits
    buffer = buffer ^ working; //XORed
  }
  return buffer;
}

void GPS_poling(byte Target, byte Rate){ //Divisor, 0 = off, 1-?, 0-9 in this implementation
  //Targets:
  // 0 GGA
  // 1 GLL
  // 2 GSA
  // 3 GSV
  // 4 RMC
  // 5 VTG
  char cRate[10] = {'0','1','2','3','4','5','6','7','8','9'};
  //char cTarget[6] = {'0','1','2','3','4','5'};
  char cTargetA[6] = {'G','G','G','G','R','V'};
  char cTargetB[6] = {'G','L','S','S','M','T'};
  char cTargetC[6] = {'A','L','A','V','C','G'};
  char message[24] = "PUBX,40,GGA,0,0,0,0,0,0";
  if(Rate>9){Rate=9;}
  if(Rate<0){Rate=0;}
  //itoa(Rate, cRate,10);
  message[14]=cRate[Rate];
  message[8] = cTargetA[int(Target)];
  message[9] = cTargetB[int(Target)];
  message[10] = cTargetC[int(Target)];
  byte checksum = NMEA_checksum(message, 19);

  Serial2.write('$');
  for(int i = 0; i < 19; i++){
    Serial2.write(message[i]);
  }

  Serial2.write('*');
  Serial2.print(checksum,HEX);
  Serial2.write(0x0d);
  Serial2.write(0x0a);


}

void GPS_request(byte Target){
  //Targets:
  // 0 GGA
  // 1 GLL
  // 2 GSA
  // 3 GSV
  // 4 RMC
  // 5 VTG
  if(Target>5){return;}
  char cTargetA[6] = {'G','G','G','G','R','V'};
  char cTargetB[6] = {'G','L','S','S','M','T'};
  char cTargetC[6] = {'A','L','A','V','C','G'};
  char message[10] = "EIGPQ,GGA";
  message[6] = cTargetA[int(Target)];
  message[7] = cTargetB[int(Target)];
  message[8] = cTargetC[int(Target)];
  byte checksum = NMEA_checksum(message, 9);

  Serial2.write('$');
  for(int i = 0; i < 9; i++){
    Serial2.write(message[i]);
  }

  Serial2.write('*');
  Serial2.print(checksum,HEX);
  Serial2.write(0x0d);
  Serial2.write(0x0a);

}

void GPS_Query_Time(){
  if(GPSTime[0]!='9'){
    serial_write(FEND);
    serial_write(CMD_GPS_CMD);
    serial_write(GPS_CMD_TIME);
    for(int i = 0; i < 6; i++){
      serial_write(GPSTime[i]);
    }
    serial_write(FEND);
    //debug
    serial_write(char(13));
    serial_write(char(10));
    //eng debug
  }
  else{//if time not buffered
    isGPS_Waiting_Time = true;
  }
}

void GPS_Query_Location(){
    if(GPS_Lon[0]=='0'||GPS_Lon[0]=='1'){
      serial_write(' ');
      serial_write(GPS_Lat[0]);
      serial_write(GPS_Lat[1]);
      serial_write(' ');
      serial_write(GPS_Lat[2]);
      serial_write(GPS_Lat[3]);
      serial_write('.');
      serial_write(GPS_Lat[4]);
      serial_write(GPS_Lat[5]);
      serial_write(GPS_Lat[6]);
      serial_write('\'');
      serial_write(' ');
      serial_write(GPS_Lat[7]);
      serial_write(char(13));
      serial_write(char(10));

      serial_write(GPS_Lon[0]);
      serial_write(GPS_Lon[1]);
      serial_write(GPS_Lon[2]);
      serial_write(' ');
      serial_write(GPS_Lon[3]);
      serial_write(GPS_Lon[4]);
      serial_write('.');
      serial_write(GPS_Lon[5]);
      serial_write(GPS_Lon[6]);
      serial_write(GPS_Lon[7]);
      serial_write('\'');
      serial_write(' ');
      serial_write(GPS_Lon[8]);
      serial_write(char(13));
      serial_write(char(10));
    }
  else{
    isGPS_Waiting_Location = true;
  }
}


void GPS_Parse_Time(){
  if(GPSGGA[8]==','){return;}
  for(int i = 0; i < 6; i++){
    GPSTime[i] = GPSGGA[8+i];
  }
}

void GPS_Parse_Location(){
  if(GPSGGA[18]!=','){
    if(GPSGGA[31]=='0'||GPSGGA[31]=='1'){
      GPS_Lat[0] = GPSGGA[18];
      GPS_Lat[1] = GPSGGA[19];
      GPS_Lat[2] = GPSGGA[20];
      GPS_Lat[3] = GPSGGA[21];
      GPS_Lat[4] = GPSGGA[23];
      GPS_Lat[5] = GPSGGA[24];
      GPS_Lat[6] = GPSGGA[25];
      GPS_Lat[7] = GPSGGA[29];

      GPS_Lon[0] = GPSGGA[31];
      GPS_Lon[1] = GPSGGA[32];
      GPS_Lon[2] = GPSGGA[33];
      GPS_Lon[3] = GPSGGA[34];
      GPS_Lon[4] = GPSGGA[35];
      GPS_Lon[5] = GPSGGA[37];
      GPS_Lon[6] = GPSGGA[38];
      GPS_Lon[7] = GPSGGA[39];
      GPS_Lon[8] = GPSGGA[43];
    }
  }

 

}

void GPS_init(){
    Serial2.begin(GPS_BAUD_RATE, SERIAL_8N1, PIN_GPS_RX, PIN_GPS_TX);
    GPS_poling(0,0);
    GPS_poling(1,0);
    GPS_poling(2,0);
    GPS_poling(3,0);
    GPS_poling(4,0);
    GPS_poling(5,0);

}

void GPS_sweep(){
  char coi[1];
  Serial2.readBytes(coi,1);
  if(coi[0] == char(13)){

    if(GPS_Buffer[4]=='G'&&GPS_Buffer[5]=='G'&&GPS_Buffer[6]=='A'){
      memset(GPSGGA,0xFF,100);
      strncpy(GPSGGA,GPS_Buffer,GPS_Index);
      GPS_Parse_Time();
      GPS_Parse_Location();
      if(isGPS_Waiting_Time){
        isGPS_Waiting_Time = false;
        GPS_Query_Time();
      }
      if(isGPS_Waiting_Location){
        isGPS_Waiting_Location = false;
        GPS_Query_Location();
      }
    }
    if(GPS_Buffer[4]=='G'&&GPS_Buffer[5]=='L'&&GPS_Buffer[6]=='L'){
      memset(GPSGLL,0xFF,100);
      strncpy(GPSGLL,GPS_Buffer,GPS_Index);
    }
    if(GPS_Buffer[4]=='V'&&GPS_Buffer[5]=='T'&&GPS_Buffer[6]=='G'){
      memset(GPSVTG,0xFF,100);
      strncpy(GPSVTG,GPS_Buffer,GPS_Index);
    }
    if(GPS_Buffer[4]=='R'&&GPS_Buffer[5]=='M'&&GPS_Buffer[6]=='C'){
      memset(GPSRMC,0xFF,100);
      strncpy(GPSRMC,GPS_Buffer,GPS_Index);
    }
    GPS_Index = 0;
    memset(GPS_Buffer,0xFF,100);
  }
  else {
    GPS_Buffer[GPS_Index] = coi[0];
    GPS_Index++;
  }

  if(GPS_Index > 98){
    //Serial.println("ERROR");
    //Serial.println("Buffer Overflow: GPS");
    GPS_Index=0;
    memset(GPS_Buffer,0xFF,100);
  }
    
}







void GPS_SetAutomatic(bool state)
{
  isGPS_Automated = state;
}

// Updates all useful buffers at full rate
void GPS_Go_Hot(){
    GPS_poling(0,1);
    GPS_poling(1,1);
    GPS_poling(2,0);
    GPS_poling(3,0);
    GPS_poling(4,1);
    GPS_poling(5,1);
}

// Shuts down automated buffering
void GPS_Go_Cold(){
    GPS_poling(0,0);
    GPS_poling(1,0);
    GPS_poling(2,0);
    GPS_poling(3,0);
    GPS_poling(4,0);
    GPS_poling(5,0);
}

void GPS_Enable_Heartbeat(bool state){
  if(state){
    GPS_poling(0,1);
    GPS_SetAutomatic(true);
  }
  else
  {
    GPS_poling(0,0);
    GPS_SetAutomatic(false);
  }

}

void GPS_send(){
  if(GPSGGA[1]=='$'){
    byte i = 0;
    char coi;
    serial_write(FEND);
    serial_write(CMD_GPS_CMD);
    serial_write(GPS_CMD_SENTENCE);
    while(coi != 0xFF){
      coi = GPSGGA[i];
      if(coi != 0xFF){serial_write(coi);}
      i++;
      if(i>99){break;}
    }
    serial_write(FEND);
//    Serial.println("");
//    GPSGGA[0]=0;
    memset(GPSGGA,0xFF,100);
  }

  if(GPSGLL[1]=='$'){
    byte i = 0;
    char coi;
    serial_write(FEND);
    serial_write(CMD_GPS_CMD);
    serial_write(GPS_CMD_SENTENCE);
    while(coi != 0xFF){
      coi = GPSGLL[i];
      if(coi != 0xFF){serial_write(coi);}
      i++;
      if(i>99){break;}
    }
    serial_write(FEND);
//    GPSGLL[0]=0;
    memset(GPSGLL,0xFF,100);
  }

  if(GPSVTG[1]=='$'){
    byte i = 0;
    char coi;
    serial_write(FEND);
    serial_write(CMD_GPS_CMD);
    serial_write(GPS_CMD_SENTENCE);
    while(coi != 0xFF){
      coi = GPSVTG[i];
      if(coi != 0xFF){serial_write(coi);}
      i++;
      if(i>99){break;}
    } 
    serial_write(FEND);
//    GPSVTG[0]=0;
    memset(GPSVTG,0xFF,100);
  }

  if(GPSRMC[1]=='$'){
    byte i = 0;
    char coi;
    serial_write(FEND);
    serial_write(CMD_GPS_CMD);
    serial_write(GPS_CMD_SENTENCE);
    while(coi != 0xFF){
      coi = GPSRMC[i];
      if(coi != 0xFF){serial_write(coi);}
      i++;
      if(i>99){break;}
    }
    serial_write(FEND);
//    GPSRMC[0]=0;
    memset(GPSRMC,0xFF,100);
  }
}

void kiss_GPS_send(){
   GPS_send();
}

void kiss_GPS_location(){

}

void kiss_GPS_time(){

}

void GPS_Query_NMEA(){
  GPS_send();
}


void GPS_process(){
  GPS_sweep();
  if(isGPS_Automated || isGPS_Waiting_NMEA){
    GPS_send();
  }
}

void GPS_handler(byte CMD){
  if(CMD==GPS_CMD_TIME){
    GPS_Query_Time();
  }
  if(CMD== GPS_CMD_LOCATION){
    GPS_Query_Location();
  }
  if(CMD==GPS_CMD_SENTENCE){
    GPS_Query_NMEA();
  }
  if(CMD==GPS_CMD_ENABLE_HEARTBEAT){
    GPS_Enable_Heartbeat(true);
  }
  if(CMD==GPS_CMD_DISABLE_HEARTBEAT){
    GPS_Enable_Heartbeat(false);
  }
  if(CMD==GPS_CMD_HOT){
    GPS_Go_Hot();
  }
  if(CMD==GPS_CMD_COLD){
    GPS_Go_Cold();
  }
  if(CMD==GPS_CMD_ENABLE_AUTOMATIC){
    GPS_SetAutomatic(true);
  }
  if(CMD==GPS_CMD_DISABLE_AUTOMATIC){
    GPS_SetAutomatic(false);
  }

}