// type desc,time,latitude,longitude,altitude (m),speed (km/h),course,pitch,roll
char myStr1[] = "T,1653140256,44.0752182,9.9014883,0,7.77,100,4,11";
char myStr2[] = "T,1653140257,44.0752106,9.9015102,0,6.66,95,0,6";

// Buffer per i dati provenienti dalla seriale
char rxBuff[128];

float latitude, longitude, speed;
int altitude, course, pitch, roll;

void parseLine(const char* myStr) {
  // Usiamo una copia (strtok modifica la stringa)
  char str[strlen(myStr)];
  strcpy(str, myStr);

  // Variabile usata per lo switch/case
  int index = 0;

  // Trova la prima occorrenza di ',' e salva il puntatore nella variabile token
  char* token = strtok(str, ",");

  while (token != NULL )
  {
    switch (index) {
      default:
        break;
      case 5:  // speed
        speed = atof(token);        
        break;
      case 6:  // course
        course = atoi(token);        
        break;
      case 7:  // pitch
        pitch = atoi(token);        
        break;
      case 8:  // roll
        roll = atoi(token);        
        break;
    }
    // Passiamo al prossimo valore
    index++;
    // Continua cercando i token successivi
    token = strtok(NULL, ",");
  }

}


void editLine(char* str) {
  parseLine(str);
  int count = 0;
  for (int i = 0; i < strlen(str); i++) {        
    
    if( str[i] == ',')
      count++;

    // Se sono arrivato alla quinta virgola, devo inserire i valori modificati
    if (count == 5) {
      // Se sono arrivato alla posizione corrispondente alla velocità,
      // inserisco i dati formattati tenendo conto dell'offset iniziale.
      snprintf(str + i + 1, strlen(str), 
        "%02d.%02d,%+04d,%+04d,%+04d",  
        (int)speed, (int)(speed*100)%100, 
        course, pitch, roll
      );
      break;
    }
  }    
}


bool readSerial() {
  static uint16_t pos = 0;
  while(Serial.available()) {
    char ch =  (char) Serial.read();   
    rxBuff[pos++] = ch;

    if (ch == '\n') {
      rxBuff[pos] = '\0';
      pos = 0;
      return true;
    }
  }

  return false;
}

void setup()
{
  Serial.begin(115200);
  Serial.println("START");
  Serial.println();

  // Simuliamo la lettura di una riga dalla seriale
  strcpy(rxBuff, myStr1);
  Serial.println("Originale: ");
  Serial.println(rxBuff);
  Serial.println("Modificata: ");
  editLine(rxBuff);
  Serial.println(rxBuff);

  // strcpy(rxBuff, myStr2);
  // Serial.println("Originale: ");
  // Serial.println(rxBuff);
  // Serial.println("Modificata: ");
  // editLine(rxBuff);
  // Serial.println(rxBuff);

}

void loop()
{

  // per provare con il terminale, copia e incolla il contenuto di myStr1 o di myStr2
  if (readSerial() ) {
   if (rxBuff[0] == 'T') {
    Serial.println("\nRicevuto: ");
    Serial.print(rxBuff); // Il carattere '\n' di nuova linea è già contenuto in rxBuff
    editLine(rxBuff);
    Serial.println("Risultato: ");
    Serial.println(rxBuff);
   }
 }

}