#include <CSV_Parser.h>
#include <SPI.h>
#include <SD.h>


//const int chipSelect = 10;
void Motor1(int d, int t);
void Motor2(int d, int t);
void halt();  //stop everything
void setup() {

//motor drive pins
pinMode(20, OUTPUT);
pinMode(21, OUTPUT);

pinMode(22, OUTPUT);
pinMode(23, OUTPUT);

  Serial.begin(9600);

  delay(5000);

  Serial.print("Initializing SD card...");
  
  // see if the card is present and can be initialized:
  if (!SD.begin(BUILTIN_SDCARD)) {
    Serial.println("Card failed, or not present");
    
    // don't do anything more:
    while (1);
  }
  Serial.println("card initialized.");


  CSV_Parser cp(/*format*/ "dd", /*has_header*/ false, /*delimiter*/ ',');

  // The line below (readSDfile) wouldn't work if SD.begin wasn't called before.
  // readSDfile can be used as conditional, it returns 'false' if the file does not exist.
  if (cp.readSDfile("/nav0.txt")) {
    int16_t *column_1 = (int16_t*)cp["column_1"];
    int16_t *column_2 = (int16_t*)cp["column_2"];

    if (column_1 && column_2) {
      for(int row = 0; row < cp.getRowsCount(); row++) {
        Serial.print("row = ");
        Serial.print(row, DEC);
        Serial.print(", column_1 = ");
        Serial.print(column_1[row], DEC);
        Serial.print(", column_2 = ");
        Serial.println(column_2[row], DEC);
      }
    } else {
      Serial.println("ERROR: At least 1 of the columns was not found, something went wrong.");
    }
    
    // output parsed values (allows to check that the file was parsed correctly)
    cp.print(); // assumes that "Serial.begin()" was called before (otherwise it won't work)
    
  } else {
    Serial.println("ERROR: File called '/nav.txt' does not exist...");
  }
}

void loop() {
  //+int fwd -int reverse 0 stop
  //fuuuq if halt() is used, I cant drive 2 motors concurently?
Motor1(1,50);
Motor2(1,50); 
//ok use x,y cords as motor timing. get direction from.... diff of current number-previous number (x,y - x1,y1) x is 2 reads before x1

//delay(10000);
}

void Motor1(int d, int t){ //right
//right rev 20
//right fwd 21
if(d==0){
halt();
}
if(d>=1){
    for(int i=0; i<t; i++){
      digitalWrite(21,HIGH);
      delay(i);
      Serial.println(i);
      }
      halt();  
      }
  if(d<0){
    //reverse
    for(int i=0; i<t; i++){
      digitalWrite(20,HIGH);
      delay(i);
      Serial.println(i);
      }
      halt();    
    }
     halt(); 
  }
  
void Motor2(int d, int t){ // left
  //run then stop
//left fwd 22
//left rev 23
if(d==0){
halt();
}
if(d>=1){
    for(int i=0; i<t; i++){
      digitalWrite(22,HIGH);
      delay(i);
      }
      halt();  
      }
  if(d<0){
    //reverse
    for(int i=0; i<t; i++){
      digitalWrite(23,HIGH);
      delay(i);
      }
      halt();    
    }
     halt();
    }

void halt(){
digitalWrite(20,LOW);
digitalWrite(21,LOW);
digitalWrite(22,LOW);
digitalWrite(23,LOW);
delay(100);  
  }