#include "WProgram.h"
/* MultiServoMultiUltrasonic
 * -------------------------
 *
 * MultiServoControl this application tries to control multiple servos.
 * Code avoids use of delays. Non blocking code.
 * 
 * (CC copyleft no rights) 2007 by KIILO
 * <http://kiilo.org>
 * <mailto:kiilo@kiilo.org>
 *
 */

// variable declaration
void SerialParser(void);
void CallHandler(void);
void UPing(int UPingN);
void SerialOut();
void setup();
void loop();
int Step = 25;                             // stepping increment
int Pos = 0;



// UltraSonic variables
int UPingPin[1] = {  8 };                  // Ultrasonic pins
int UPingTime[1] = {  0 };              // ping timer
int val = 0;

byte SerIn = '@';
byte SerOut = '@';
byte VarHandlerChar = '@';
int VarN[4] = {0 ,0 ,0 ,0};
int PosN = -1;
int VarSign = 1; 

// SERIAL PARSER **********************************
void SerialParser(void) {
  while (Serial.available()) {
    //SerIn = '@';
    SerIn = Serial.read();
    Serial.print(SerIn, BYTE);
    // debugging CODE
    // Serial.print(' ');
    // Serial.print(SerIn);
    if ((SerIn >= 65) && (SerIn <= 90)) {          // if ASCII 'A' - 'Z'
      VarHandlerChar = SerIn;  
      for (PosN = 0; PosN < 8; PosN++) {           // Aufr\u2030umen
        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;
      CallHandler();                               // Call funktion by first Char
    }   // IfEND CR spooky things
  }
}

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

void CallHandler(void) {
  switch(VarHandlerChar) {
      
    //case 'A':  // do something when B
    //  
    //  break;
  case 'M':  // Manual Set Temp
    
    break; 
  case 'D':   // Debug eIn/aus
   
    break;
  case 'T':  // Debug AUsgabe
   
    break; 
  } // Switch END
}

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

// function declaration

// get a distance 
void UPing(int UPingN) {
  // send ping pulse
  UPingTime[UPingN] = 0;
  pinMode(UPingPin[UPingN], OUTPUT);      // Switch signalpin to output
  digitalWrite(UPingPin[UPingN], LOW);   // Send low pulse 
  delayMicroseconds(2);                            // Wait for 2 microseconds
  digitalWrite(UPingPin[UPingN], HIGH);  // Send high pulse 
  delayMicroseconds(5);                            // Wait for 5 microseconds
  digitalWrite(UPingPin[UPingN], LOW);   // Holdoff
  // Listening for echo pulse
  pinMode(UPingPin[UPingN], INPUT);      // Switch signalpin to input
  val = digitalRead(UPingPin[UPingN]);   // Append signal value to val
  while(val == LOW) {                    // Loop until pin reads a high value
    val = digitalRead(UPingPin[UPingN]);
  }
  while(val == HIGH) {                   // Loop until pin reads a low value
    val = digitalRead(UPingPin[UPingN]);
    UPingTime[UPingN]++;            // Count echo pulse time
  }
}





void SerialOut() {
  Serial.print("D ");
  Serial.print(UPingTime[0]);
  Serial.println(" !");

}




// end functions declaration

// setup
void setup() {
  // initialize inputs/outputs
  Serial.begin(57600);                       // Sets the baud rate to 9600
  pinMode(UPingPin[0], OUTPUT);            // Sets UPing[1]
  digitalWrite(UPingPin[0], LOW);
}

// main loop
void loop() {
  UPing(0);
  SerialOut();
  delay(20);  
}

int main(void)
{
	init();

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