#include "WProgram.h"

// variable declaration
// 
void SerialParser(void);
void CallHandler(char HandlerChar);
void Forward();
void Backward();
void Stop();
void Left();
void Right();
void Straight();
void Timing();
void Velocity();
void Function_Char(char InChar);
void setup(void);
void loop(void);
int i = 0;              // i counter 
int VarSign = 1;
int SerIn = -1;          // incoming byte from serial RX
int VarN[4] = { 
  0, 0, 0, 0 };      // number array
int PosN = -1;       // index counter
char VarHandlerChar = '@';       // 
unsigned long DebugTime = 0;
byte Motor1Pin = 8;
byte Motor2Pin = 9;
byte Steer1Pin = 10;
byte Steer2Pin = 11;

long unsigned Cycles = 0;
long unsigned AnalogReadRefreshTime = 0;
int AnalogReadRefreshCycle = 1000;
int LDR1 = 0;

// function declaration
// type name_of_function(type parameters) {
//   code_block;
//  }

// SERIAL PARSER **********************************
void SerialParser(void) {

  while (Serial.available() > 0) {                         // while there is serial data 
    digitalWrite( 13, HIGH);
    SerIn = '@';
    SerIn = Serial.read();
    Serial.print(SerIn, BYTE); // just print out what you get (daisy chain idea) 
    // debugging CODE
    // Serial.print(' ');
    // Serial.print(SerIn, HEX);
    if ((SerIn >= 65) && (SerIn <= 90)) {          // if ASCII 'A' - 'Z'
      VarHandlerChar = SerIn;  
      for (PosN = 0; PosN < 9; PosN++) {           // Aufr\u00e4umen
        VarN[PosN] =  0;                           // reset Var array
      }  
      PosN = -1;                              
    }
    if (SerIn == 45) {
      VarSign = -1;
    }
    if ((SerIn >= 48) && (SerIn <= 57)) {          // if ASCII numeric '0' - '9'
      VarN[PosN] = VarN[PosN] * 10 + (SerIn - 48);
    }
    if (SerIn == ' ') {                            // if ASCII " " detected
      if (PosN > -1) {
        VarN[PosN] = VarSign * VarN[PosN];           // Assign SIgn 
        VarSign = 1;
      }
      PosN++;
    }
    if (SerIn == 33 || SerIn == 10 || SerIn == 13) {  // '!' or CR or LF ends all this spooky things ...
      // VarN[PosN] = VarSign * VarN[PosN];           // Assign Sign
      //VarSign = 1;
      digitalWrite(13, LOW);
      CallHandler(VarHandlerChar);                               // Call funktion by first Char      
      break;                                       // break it here to give other functions a chance 
    }   // IfEND CR spooky things
  }
}

// CALLHANDLER SCHEDULER ****************************** 
// which of YOUR functions are called?
//

void CallHandler(char HandlerChar) {
  switch(HandlerChar) {
  case 'F':    //  
    Forward();
    break;    
  case 'B':    // do something when B
    Backward();
    break;
  case 'L':    // 
    Left();
    break;
  case 'R':    // S 
    Right();
    break;
  case 'M':    // M
    Straight();
    break;
  case 'S':    // S 
    Stop();
    break;    
  case 'T':    // S 
    Timing();
    break;
  case 'V':    // S 
    Velocity();
    break;
  } // Switch END
  return;
}

//
// CALLHANDLER FUNCTIONS *******************************
// YOUR funktions here:

void Forward() {
  digitalWrite(Motor1Pin, HIGH);
  if ((VarN[0] > 0) && (VarN[0] < 256)) {
    analogWrite(Motor2Pin, 255 - VarN[0]);
  }
  else {
    digitalWrite(Motor2Pin, LOW);
  }
}

void Backward() {
  digitalWrite(Motor1Pin, LOW);
  if ((VarN[0] > 0) && (VarN[0] < 256)) {
    analogWrite(Motor2Pin, VarN[0]);
  }
  else {
    digitalWrite(Motor2Pin, HIGH);
  }
}

void Stop() {
  digitalWrite(Motor1Pin, LOW);
  digitalWrite(Motor2Pin, LOW);
}
void Left() {
  digitalWrite(Steer1Pin, HIGH);
  digitalWrite(Steer2Pin, LOW);
}

void Right() {
  digitalWrite(Steer1Pin, LOW);
  digitalWrite(Steer2Pin, HIGH);
}

void Straight() {
  digitalWrite(Steer1Pin, LOW);
  digitalWrite(Steer2Pin, LOW);
}

void Timing() {
  if ((VarN[0] > 10) && (VarN[0] < 5000)) {
    AnalogReadRefreshCycle = VarN[0];
  }
}

void Velocity() {
  if ((VarN[0] > -1) & (VarN[0] < 256)) {
    analogWrite(Motor2Pin, VarN[0]);
  }
}

void Function_Char(char InChar) {
  Serial.print(InChar);
  Serial.print(' ');
  for (int i = 0; i < PosN; i++) { // the array back to you.
    Serial.print(VarN[i]);
    Serial.print(' ');
  }
  Serial.println('!');
}

// MY FUNCTIONS ****************************************


// SETUP ***********************************************

void setup(void) {
  pinMode(Motor1Pin, OUTPUT);
  digitalWrite(Motor1Pin, LOW);
  pinMode(Motor2Pin, OUTPUT);
  digitalWrite(Motor2Pin, LOW);
  pinMode(Steer1Pin, OUTPUT);
  digitalWrite(Steer1Pin,LOW);
  pinMode(Steer2Pin, OUTPUT);
  digitalWrite(Steer2Pin, LOW);
  pinMode(13, OUTPUT);
  // initialize inputs/outputs
  // start serial port, 115200 8-N-1:
  Serial.begin(57600);
}


// LOOP ************************************************

void loop(void) {  
  // update Servo 
  // Get Serial Data
  SerialParser(); 
  if (millis() > AnalogReadRefreshTime) {  
    AnalogReadRefreshTime = millis() + AnalogReadRefreshCycle;
    Cycles++;
    //LDR1 = analogRead(0);
    Serial.print("V ");
    Serial.print(Cycles);
    Serial.println(" !"); 
  }
}

int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}