/*
 * DIRR => Inkrements left
 * DIRL => Inkrements right
 * CNT => returns current position
 * IMPXXX => set Impulses davor muss DIRX angegeben werden33
 */

String inputString = "";         // a String to hold incoming data
bool stringComplete = false;  // whether the string is complete


const int trackA =  2;
const int trackB =  8;

int state_trackA = LOW;
int state_trackB = LOW;  

unsigned long previousMillis = 0;
unsigned long currentMillis;

unsigned long counter = 0;  //Zähler der impulse
unsigned long prevcounter = 0;
unsigned long impuls = 0; //Anzahl impulse
long interval = 80; //Intervall der impulse 1333=3m/s; 1000=1m/s; 50000 = 0,01m/s

boolean blinking = false;

boolean outputA = false;
boolean outputB = false;

String msg;
int stringlength;
int delayPULS = 50; //Dauer einzelimpuls in ms
int shift = 1;// Verschiebung trackA zu trackB => links/rechts Fahrt


void setup() {
  //currentMillis = millis();
  // initialize serial:
  Serial.begin(9600);
  // reserve 200 bytes for the inputString:
  inputString.reserve(200);
    pinMode(trackA, OUTPUT);
    pinMode(trackB, OUTPUT);
  /*while (!Serial) {
    ;
  }*/
  outputA = true;
    outputB = true;
}

void loop() {

  
  //currentMillis = millis();
  currentMillis = micros();
  // print the string when a newline arrives:
  if (stringComplete) {
    //Serial.println("received:  " + inputString); 
   

    msg = inputString;
    // clear the string:
    inputString = "";
    stringComplete = false;
    stringlength = msg.length();

  if (msg.substring(0,3) == "INT") {
      String value = msg.substring(3,stringlength-1);
      Serial.print("INT"); 
      
      interval = value.toInt();
      Serial.println(value); 
      if (interval == 0){
        outputA = false;
        outputB = false;
      }
      msg="";
  }
  if (msg.substring(0,3) == "IMP") {
      String value = msg.substring(3,stringlength-1);
      Serial.print("IMP"); 

      impuls = value.toInt();
            Serial.println(value); 
      counter = 0;
      if (interval == 0){
        outputA = false;
        outputB = false;
      }
      msg="";
  }
  if (msg.substring(0,2) == "ON") { //Anschalten
    outputA = true;
    outputB = true;
    Serial.println("ON"); 
    msg="";
  }
  if (msg.substring(0,3) == "OFF") {  //Ausschalten
    outputA = false;
    outputB = false;
    Serial.println("OFF");
    msg="";
  }
  if (msg.substring(0,3) == "CNT") {  //Rückgabe Zähler
    Serial.print("CNT");
    Serial.println(counter);
    msg="";
  }
  if (msg.substring(0,4) == "PULS") { //kurzer Impuls
    Serial.println("PULS");
    digitalWrite(trackA, true);
    counter ++;
    msg="";
    delay(delayPULS);
        digitalWrite(trackA, false);
  }
  if (msg.substring(0,3) == "RST") { //Reset Zähler
    Serial.println("RST");
    counter = 0;
    msg="";
  }
  if (msg.substring(0,4) == "DIRR") { //Anschalten
    Serial.println("DIRR");
    shift = false;
    msg="";
  }
    if (msg.substring(0,4) == "DIRL") { //Anschalten
    Serial.println("DIRL");
    shift = true;
    msg="";
  }
  

  }

  if (impuls != 0 && counter > impuls-1  &&  outputA == true){
    outputA = false;
    Serial.print("IMP done: ");
     Serial.println(counter);
  }else if(counter > impuls  && impuls != 0 && outputB == true){
    outputB == false;
  }
  //trackA
  if (currentMillis - previousMillis > interval && outputA == true) {
    
    previousMillis = currentMillis;
    if (state_trackA == LOW) {
      counter ++;
      state_trackA = HIGH;
    } else {
      state_trackA = LOW;
    }
    digitalWrite(trackA, state_trackA);
  }else if(currentMillis - previousMillis > interval && outputA == false){    
    digitalWrite(trackA, false);
  }  
  //trackB
  if (currentMillis - previousMillis > interval*0.5 && outputB == true) {
    if (state_trackA == shift) {
      state_trackB = HIGH;
    } else {
      state_trackB = LOW;
    }
    digitalWrite(trackB, state_trackB);
  }else if(currentMillis - previousMillis > interval*0.5 && outputB == false){    
    digitalWrite(trackB, false);
  }
  


  
}

void serialEvent() {
  while (Serial.available()) {
    // get the new byte:
    char inChar = (char)Serial.read();
    // add it to the inputString:
    inputString += inChar;
    // if the incoming character is a newline, set a flag so the main loop can
    // do something about it:
    if (inChar == '\n') {
      stringComplete = true;
    }
  }
}