#include <SPI.h>
#include <SD.h>
char mes;
String mes1;
int foundpos;
int startpos=0;
String target="$PTAHADC";
//int pospoint=0;
//int colpoint=0;
//int startpoint=0;
int last=0;
char* data1[6];
int n=0;
float time,d1,d2;

void setup() {
  Serial.begin(115200);
  if(!SD.begin(D3)){
    Serial.println("Fail");
    while(1);
  }
  Serial.println("Connect");
  File data = SD.open("test.txt", FILE_WRITE);
  data.print("$PTAHADA,B,49899,49899,49899,-26.1,0.0,0.0,84.9*13");
  data.print("$PTAHADC,B,0.0,30.8,3.283,-0.444*3D");
  data.print("$PTAHADV,B,0.0,0.0,0.000,0.0088*00");
  data.print("$PTAHADA,B,49899,49899,49899,-17.1,0.0,0.0,84.9*11");
  data.print("$PTAHADC,B,0.0,30.8,3.283,-0.444*3D");
  data.print("$PTAHADV,B,0.0,0.0,0.000,0.0088*00");
  data.print("$PTAHADA,B,49899,49899,49899,-17.2,0.0,0.0,84.9*12");
  data.print("$PTAHADC,B,0.0,30.8,3.283,-0.444*3D");
  data.print("$PTAHADV,B,0.0,0.0,0.000,0.0088*00");
  data.print("$PTAHADA,B,49899,49899,49899,-20.8,0.0,0.0,84.9*1C");
  data.print("$PTAHADC,B,0.0,30.8,3.283,-0.444*3D");
  data.print("$PTAHADV,B,0.0,0.0,0.000,0.0088*00");
  data.print("$PTAHADA,B,49899,49899,49899,-29.8,0.0,0.0,84.9*15");
  data.print("$PTAHADC,B,0.0,30.8,3.283,-0.444*3D");
  data.print("$PTAHADV,B,0.0,0.0,0.000,0.0088*00");
  data.print("$PTAHADA,B,49899,49899,49899,-28.0,0.0,0.0,84.9*1C");
  data.print("$PTAHADC,B,0.0,30.8,3.283,-0.444*3D");
  data.print("$PTAHADV,B,0.0,0.0,0.000,0.0088*00");
  data.print("$PTAHADA,B,49899,49899,49899,-28.3,0.0,0.0,84.9*1F");
  data.close();
  data = SD.open("test.txt");
  String t;
  
  if (data) {
    while (data.available()){
      mes=data.read();
      mes1+=String(mes);
    } 
    foundpos=mes1.indexOf(target,startpos);
    while (foundpos !=-1){
      //startpoint=foundpos;
      last=mes1.indexOf("*3D",foundpos);
      //Serial.println(last);
      /*while (colpoint !=3){
        pospoint=mes1.indexOf(",", startpoint);
        startpoint=pospoint+1;
        colpoint++;
        
      }*/
      //colpoint=0;
      t=mes1.substring(foundpos, last);
      startpos=foundpos + target.length();
      foundpos=mes1.indexOf(target, startpos);
      
      char filstrok[t.length()+1];
      t.toCharArray(filstrok, t.length()+1);
      char *token = strtok(filstrok, ",");
      while(token !=NULL){
        data1[n]=token;
        token=strtok(NULL,",");
        n++;
      }
      time=atof(data1[3]);
      d1=atof(data1[4]);
      d2=atof(data1[5]);
      Serial.println(time);
      n=0; 
    }
    data.close();
  }
  else{
    Serial.println("Error open");
    data.close();
  }
  if(SD.exists("test.txt")){
    Serial.println("exist");
  }
  else{
    Serial.println("not");
  }
}

void loop() {


}