// 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;";

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

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


void parseLine(char* str) {
  // Array di caratteri per la stringa formattata da stampare
  char charBuff[32];

  // 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, ",");

  Serial.println();
  while (token != NULL )  {

    switch (index) {
      case 0:   // type desc
      case 1:   //  timestamp
        break;
      case 2:   // latidude
        latitude = atof(token);
        Serial.print("Latitude ");
        Serial.println(latitude, 7);
        break;
      case 3:  // longitude
        longitude = atof(token);
        Serial.print("Longitude ");
        Serial.println(longitude, 7);
        break;
      case 4:  // altitude
        altitude = atoi(token);
        snprintf(charBuff, sizeof(charBuff), "Altitude %+04d", altitude);
        Serial.println(charBuff);
        break;
      case 5:  // speed
        speed = atof(token);
        snprintf(charBuff, sizeof(charBuff), "Speed %02d.%02d", (int)speed, (int)(speed*100)%100);
        //dtostrf(speed, 5, 2, charBuff);
        Serial.print("Speed ");
        Serial.println(charBuff);
        break;
      case 6:  // course
        course = atoi(token);
        snprintf(charBuff, sizeof(charBuff), "Course %+04d", course);
        Serial.println(charBuff);
        break;
      case 7:  // pitch
        pitch = atoi(token);
        snprintf(charBuff, sizeof(charBuff), "Pitch %+04d", pitch);
        Serial.println(charBuff);
        break;
      case 8:  // roll
        roll = atoi(token);
        snprintf(charBuff, sizeof(charBuff), "Roll %+04d", roll);
        Serial.println(charBuff);
        break;
    }
    // Passiamo al prossimo valore da acquisire
    index++;
    // Continua cercando i token successivi
    token = strtok(NULL, ",");
  }
}



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("Copia e incolla il contenuto di myStr1 o di myStr2 per testare questo sketch. Es: ");
  Serial.println(myStr1);

  // parseLine(myStr1);
  // parseLine(myStr2);
}

void loop()
{

  // per provare con il terminale, copia e incolla il contenuto di myStr1 o di myStr2
  if (readSerial() ) {
    if (rxBuff[0] == 'T') {
      parseLine(rxBuff);      
    }
  }

}