char inByte, buf[100];
char *ptr,*ID,*UTC,*latitude,*NS,*longitude,*EW,*fix,*satellites,*dilation,*altitude,*units, *validity;
uint8_t len = 0;

// Struct to store each point data 

typedef struct {
  float lat; 
  float lon; 
  float alt; 
  int year;
  int month;
  int day; 
  int hrs;
  int mins; 
  float secs; 
} gps_p; 

void setup_gps() {
  Serial1.begin(9600);
}

gps_p create_gps(float lat, float lon, float alt, int year, int month, int day, int hrs, int mins, float secs) {
  gps_p new_gps;
  new_gps.lat = lat;
  new_gps.lon = lon;
  new_gps.alt = alt;
  new_gps.year = year; 
  new_gps.month = month; 
  new_gps.day = day; 
  new_gps.hrs = hrs;
  new_gps.mins = mins;
  new_gps.secs = secs;
  return new_gps;
}

// Function to print the values of a gps_p structure using printf
void print_gps(const gps_p point) {
  printf("Latitude: %f\n", point.lat);
  printf("Longitude: %f\n", point.lon);
  printf("Altitude: %f\n", point.alt);
  printf("Date: %d-%d-%d", point.year, point.month, point.day); 
  printf("Time: %d hours, %d minutes, %f seconds\n", point.hrs, point.mins, point.secs);
}


gps_p parse_gps_data(const char *lat, const char *NS, const char *lon, const char *EW, const char *alt, const char *UTC) {
  
  // parse info 
  // parse latitude: ddmm.mmmm 
  float lat_deg, lat_min;
  sscanf(lat, "%2f%5f", &lat_deg, &lat_min);
  float re_lat = lat_deg + lat_min/60;
  re_lat = (*NS == 'N') ? re_lat : -re_lat; 

  // parse longitude: dddmm.mmmm 
  float lon_deg, lon_min;
  sscanf(lon, "%3f%5f", &lon_deg, &lon_min);
  float re_lon = lon_deg + lon_min/60;
  re_lon = (*EW == 'E') ? re_lon : -re_lon; 

  // parse altitude: float 
  float re_alt = atof(alt);

  // parse time UTC: hhmmss.sss
  int hrs, mins; 
  float secs;
  sscanf(UTC, "%2d%2d%6f", &hrs, &mins, &secs);
  int day = 0; 
  int month = 0; 
  int year = 0; 

  gps_p point = create_gps(re_lat, re_lon, re_alt, year, month, day, hrs, mins, secs); 
  return point; 
}



gps_p parse_gps_messages(char *buf) {
  // Exmple Valid Message with the data wanted: 
  // Format: $GNRMC, utc, pos status, lat, lat dir, lon, lon dir, speed Kn, track true, date, mag var, var dir, mode ind
  // Format: $GNRMC, hhmmss.ss, A/V, DDmm.mm, N/S, DDDmm.mm, E/W, float, float, ddmmyy, float, E/W, A
  // GNRMC,210546.000,A,4221.6641,N,07105.5411,W,0.00,0.00,151223,,,A*62

  // 16:05:45.512 -> GPTXT,01,01,01,ANTENNA OK*35
  // 16:05:45.998 -> GNGGA,210546.000,4221.6641,N,07105.5411,W,1,05,1.5,5.1,M,0.0,M,,*63
  // 16:05:46.063 -> GNGLL,4221.6641,N,07105.5411,W,210546.000,A,A*53
  // 16:05:46.095 -> GPGSA,A,3,27,21,07,16,08,,,,,,,,2.7,1.5,2.3*3C
  // 16:05:42.144 -> BDGSA,A,3,,,,,,,,,,,,,2.7,1.5,2.3*23
  // 16:05:46.191 -> GPGSV,2,1,08,02,09,165,,07,45,308,28,08,69,193,26,09,,,22*4D
  // 16:05:46.350 -> BDGSV,1,1,00*68
  // 16:05:46.350 -> GNRMC,210546.000,A,4221.6641,N,07105.5411,W,0.00,0.00,151223,,,A*62
  // 16:05:46.415 -> GNVTG,0.00,T,,M,0.00,N,0.00,K,A*23
  // 16:05:46.478 -> GNZDA,210546.000,15,12,2023,00,00*48

  
    latitude = strtok_r(NULL,",",&ptr);
    NS = strtok_r(NULL,",",&ptr);
    longitude = strtok_r(NULL,",",&ptr);
    EW = strtok_r(NULL,",",&ptr);
    fix = strtok_r(NULL,",",&ptr);
    satellites = strtok_r(NULL,",",&ptr);
    dilation = strtok_r(NULL,",",&ptr);
    altitude = strtok_r(NULL,",",&ptr);
    units = strtok_r(NULL,",",&ptr);
    printf("UTC time: %s, ",UTC);
    printf("Latitude: %s ",latitude);
    printf("%s, ",NS);
    printf("Longitude: %s ",longitude);
    printf("%s, ",EW);
    printf("Satellites: %s, ",satellites);
    printf("Altitude: %s ",altitude);
    printf("%s\n",units);
    return parse_gps_data(latitude, NS, longitude, EW, altitude, UTC); 
}

char msg[100]; 

bool read_gps() {
  // read from port 1, send to port 0:
  if (Serial1.available()) {
    inByte = Serial1.read();
    Serial.print(inByte);
    if (inByte == '$') {
      buf[len] = '\0'; // Null-terminate the string
      Serial.print("Buffer: "); 
      Serial.println(buf);
      if (strncmp(buf,"GNGAA",5) == 0) {
        ID = strtok_r(buf,",",&ptr);
        UTC = strtok_r(NULL,",",&ptr);
        if (buf[10+UTC-buf] == ',') { // check for empty latitude
          printf("%s: no fix\n",UTC);
        } else {
        gps_p rec_point = parse_gps_messages(buf);
        }
      }
      strcpy(msg, buf); 
      memset(buf, 0, sizeof(buf)); // Clear the buffer
      len = 0; 
      return true; 
    } else {
      buf[len++] = inByte;
      if (len >= sizeof(buf)) {
        // Buffer overflow protection, reset the buffer
        memset(buf, 0, sizeof(buf));
        len = 0;
      }
    }
  }
  return false;
}