/******************************************************************************
// FreakyBot Circle
/******************************************************************************/

/******************************************************************************
/* Konfigruation
/******************************************************************************/
//pins

#define MDL 4     // Motor Direction Left
#define MSL 5     // Motor Speed Left
#define MSR 6     // Motor Speed Right
#define MDR 7     // Motor Direction Right

int IrLedVr = 3;   
int IrLedVl = 2; 
int IrLedHr = 10; 
int IrLedHl = 9;

int IrRecVr = A3;   
int IrRecVl = A2; 
int IrRecHr = A0; 
int IrRecHl = A1;
const int ledPin =  13;      // the number of the LED pin

#define DIST 65   // Define Minimum Distance

// dino
/*
#define MTRF 200  // Motor Topspeed Right Forward
#define MTRB 200  // Motor Topspeed Right Backward
#define MTLF 170  // Motor Topspeed Left  Forward
#define MTLB 180  // Motor Topspeed Left  Backward
*/

// normal
#define MTRF 200  // Motor Topspeed Right Forward
#define MTLB 170  // Motor Topspeed Left  Backward / Forw

#define MTRB 200  // Motor Topspeed Right Backward
#define MTLF 170  // Motor Topspeed Left  Forward  / Backw

// Variables
int lspeed = 0;   // current speed of left motor
int rspeed = 0;   // current speed of right motor
int spause = 1;   // wait time if a whisker was touched
long delay_time     = 0;        // time when pause was triggered
int  ledState       = LOW;      // ledState used to set the LED
long previousMillis = 0;        // will store last time LED was updated
long interval       = 5000;     // interval at which to blink (milliseconds)
bool protsta        = false;
bool motsta         = true;

/******************************************************************************
/* Setup
/******************************************************************************/
void setup () {
  //motors
  pinMode (MDL, OUTPUT);
  pinMode (MSL, OUTPUT);
  pinMode (MSR, OUTPUT);
  pinMode (MDR, OUTPUT); 
  // all IR Leds / Recevier
  pinMode(IrLedVr, OUTPUT);   
  pinMode(IrLedVl, OUTPUT); 
  pinMode(IrLedHr, OUTPUT); 
  pinMode(IrLedHl, OUTPUT);  
  pinMode(IrRecVr, INPUT);
  pinMode(IrRecVl, INPUT); 
  pinMode(IrRecHr, INPUT);
  pinMode(IrRecHl, INPUT); 
  // Serial Interface
  Serial.begin(57600); 
  Serial.println(F("Robot Starting")); 
  delay(500);
}

/******************************************************************************
/* Hauptprogramm
/******************************************************************************/
void loop () {

// left / right
for ( int i = 0; i <=73; i++ ){ 
  drive(110,165);
  delay(100);
}
      
for ( int i = 0; i <=73; i++ ){ 
  drive(165,110);
  delay(100);
}
}

/******************************************************************************
/* Funktionen
/******************************************************************************/

/******************************************************************************
/* Funktion: Drive
/******************************************************************************/
void drive( int left, int right)
{
  int lstate = LOW;
  int rstate = LOW;
 
 left = constrain(left,-255,255);
 right = constrain(right,-255,255);
 
// invert left
left = left * -1;
// invert right
//right = right * -1;
 
  if (left >= 0)
  {
    // remap the motor value to the calibrated interval
    // speed is alwas handled in the range of [-255..255]
    left = map(left,0,255,0,MTLF);
  }
  else
  {
    lstate = HIGH;
    left = 255 - map(-left,0,255,0,MTLB);
  }
  if (right >= 0)
  {
    right = map(right,0,255,0,MTRF);
  }
  else
  {
    rstate = HIGH;
    right = 255 - map(-right,0,255,0,MTRB);
  }
  analogWrite(MSL, left);
  digitalWrite(MDL, lstate);
  analogWrite(MSR, right);
  digitalWrite(MDR, rstate);

}

/******************************************************************************
 * Funktion: irRead (a,b)
 * a: Pinnummer des Ir Empfängers
 * b: Pinnummer des Ir Senders
 * c: Modus: Space = Normalmodus returns HIGH if > DIST
 *           d = Distancemodus return Distance 0 to 100 mm
 * Retourwert: HIGH = IR-Licht detektiert 
 * d.h. gemessener Wert unterscheidet sich zum Umgebungslicht
 ******************************************************************************/
int irRead(int readPin, int triggerPin, char mode)
{
    boolean zustand;
    int umgebungswert = 0;
    int irwert =0;
    float uLichtzuIr;
    
    for (int i=0;i<10;i++) {
      digitalWrite(triggerPin, LOW); 
      delay(5);
      umgebungswert = umgebungswert + analogRead(readPin);
      digitalWrite(triggerPin, HIGH); 
      delay(5);
      irwert = irwert + analogRead(readPin);
    }
    
    // Remove Umgebungswert
    irwert = irwert - umgebungswert;

    // detect obstacle
    if(irwert>DIST){
     zustand = HIGH;
    }else{
     zustand = LOW;
    }
         
     // for Debug 
     if(protsta && zustand || mode == 'd'){
/*     Serial.print("tr ");
     Serial.print(triggerPin);
     Serial.print(" r ");
     Serial.print(readPin);
     Serial.print(" um ");
     Serial.print(umgebungswert);
     Serial.print(" ir ");
     Serial.print(irwert);
     Serial.print(" % ");
     Serial.print(uLichtzuIr); */

     delay(5);
     if(mode == 'd'){
 //    Serial.print(" irwert ");
 //    Serial.print(irwert); 
     irwert = map(irwert, 0, 300, 100, -100);
 //    Serial.print(" dis ");
 //    Serial.println(irwert); 
};     
     return irwert;
	 } else {
     return zustand; 
//     Serial.print(" zu ");
//     Serial.println(zustand);
 };
  
}

// CHECKSERIAL)
//*****************************************************************************
void checkserial(){
  	if ( Serial.available() > 0 ) {
		// Read the incoming byte
		char theChar = Serial.read();
			// Parse character
		switch (theChar) {
			case 'p':
                                Serial.println(F("protocoll on/off"));
                                delay(5);
                                if (protsta){
							    protsta = false;	
							    }else{
								protsta = true;		
							    }	
                                delay(5);
                                break;
			case 'm':
                                delay(10);
                                if (motsta){
							    motsta = false;	
							    }else{
								motsta = true;		
							    }	
                                delay(5);
                                break;    			default:
				// Display error message
				Serial.print(F("ERROR: Command not found, it was: "));
				Serial.println(theChar);
				Serial.println(F("p=protocoll on/off;m=motor on/off"));
                                break;
       }
   }
   } // void checkserial