#include <Servo.h> // Import der Servo-Libraries 
  	
		  //TODO: Startknopf einbauen Eingan A3
  	
		  /** MERKZETTEL:
  	
		  malloc() nachschlagen. Koennte uns sehr mit RAM helfen.
		  Beispiel: ptr = malloc(100)  ...oder so.
  	
		  **/
  
		  // Pins
  
			// Start-Knopf
	
			const int startButton = A3;
  	
			//  Stepper - Motoren
			/* 
			   Zusatz-Infos zu den Steppern:
	   
				Pololu 1 für den Linken Stepper befindet sich direkt neben dem Arduino, 
				Pololu 2 für den Rechten Stepper ist der Andere.
				Den Linken Stepper so anbringen, dass das schwarze Kabel direkt neben dem 
				Kondensator ist, den Rechten so, dass das blaue Kabel direkt daneben ist.
			*/
	
			const int dirL = 2;   //Linker Stepper
			const int stpL = 3;   
			const int dirR = 7;  //Rechter-Stepper
			const int stpR = 9;   
			const int stepDelay = 15; // Delay zwischen steps. Steuer die Geschwindigkeit
			const double stepLength = 1.414; // Länge eines Schrittes mit den 90mm Rädern.
			boolean driveStraight = false; 
   	
			// Sonar 
	
			const int inPinSonar = A4;
			const int pingPinSonar = 12;
	
			// Arm-Servos für Drehbewegung + Heben
	
			const int rotor = 6;     //mittlerer Anschluss
			const int lifter = 4;    //innerer Anschluss
	
			// Servo mit Ventil-Funktion - 0 ist weitestmöglich Richtung geschlossen, 180 ist weitestmöglich geöffnet.
	
			const int vent = 5;  //äußerer Anschluss
			const int valveShut = 40; // Grandzahl, bei welcher der Servo vollständi geschlossen ist.
			const int valveOpen = 180;
	
			//  LEDs 
	
			const int led = 12; // LED für die Beleuchtung der Steuerungs-Phototransistoren
			const int resetLed = 13; // LED auf dem Arduino, soll Leuchten, wenn der Arduino abstürzt und sich resetted
	
			//  Phototransistoren 
	
			const int photoL = A6;
			const int photoR = A7; // Das blaue Kabel
	
			// Kontakt für Fuellstand, Erdung G56 (schwarz), Einspeisung G55 (rot)
	
			const int fuellstandsPin =A5;
	
	
			//  Konstanten
	
			const int armLength = 235; //  Armlänge in mm
			const int armOffAxis = 30; //TODO: einbauen
			const double pi= 3.15;
			const int threshold = 1020; //  für Fuellstanderkennung TODO: testen
			const int distSonar = 310; 
			const int radiusGlas = 20;
  	
  	
			//  Variablen  
	
			//  für Glaserkennung
			long length = 0; // Speichert die Anzahl der Schritte, die der Roboter an einem Glas vorbei fährt
			boolean found = false;    //Boolean, ob Roboter an einem Glas vorbei fährt
			boolean passed = false;    //Boolean, ob Roboter ein Glas passiert hat 
			int glassDistanz = 0;
			int glassCounts = 0;
   	
			//  für Fahrfunktion
			boolean rightWasLast = false;     //Boolean, ob Roboter vorher nach rechts gefahren ist
			boolean leftWasLast = false;      //Boolean, ob Roboter vorher nach links gefahren ist
			int brightnessLeft = 0;        //Wert des Phototransitoren auf der linken Seite
			int brightnessRight = 0;      //Wert des Phototransistoren auf der rechten Seite
			const int lightThreshold = 150;     // Abfrage, wie viel heller der helle Bereich ist, als die schwarze Linie
			const int lightBlack = 600;       // Lichtintensität, ab welcher wir von Erkennung der schwarzen Linie ausgehen
			const int photoCorrection = 100; // Korrektur, die auf den rechten Phototransistor angewendet wird.
	
			//  Servos
			Servo rotate;
			Servo lift;
			Servo ventil;
	
			void setup(){
	  
			  //TODO Serielle Ausgabe entfernen
			  Serial.begin(9600);
			  Serial.println("ABSTURZ oder vielleicht auch erster Start");      // Arduino-LED: Leuchtet auf, wenn der Arduino neu startet.      
	  
			  pinMode(resetLed,OUTPUT);
			  digitalWrite(resetLed,HIGH);
			  delay(100);
			  digitalWrite(resetLed,LOW);    
	  
	  
			  //Initialisierungen
	
			  // Steuerungs-LED anmachen
			  pinMode(led,OUTPUT);  
			  digitalWrite(led,HIGH);
	
			  // Arm-Servos anschließen und ausrichten
			  rotate.attach(rotor, 800, 2100);
			  lift.attach(lifter, 800, 2100);
	
			  resetArm();    
	
			  // Ventil-Servo anschließen TODO: ausrichten - benötigt wahrscheinlich nicht die Korrekturwerte
			  ventil.attach(vent);
			  ventil.write(valveShut); // Ventil schliessen. TODO Wert gegen valveShut ersetzen.
	
			  // Stepper-Motoren initialisieren
			  pinMode(dirL, OUTPUT);
			  pinMode(stpL, OUTPUT);
			  pinMode(dirR, OUTPUT);
			  pinMode(stpR, OUTPUT);
   	
			  // Stepper-Motoren Richtung auf vorwärts stellen. 
			  digitalWrite(dirL,LOW);
			  digitalWrite(dirR,LOW);
	  
			  // TODO testen, ob der Startknopf so funktioniert.
			  while(analogRead(startButton)<1020) delay(1);
	
			}
  	
		  void loop(){
				Glaserkennung(); // prüfe, ob sich ein Glas vor dem Sonar befindet.
				/*
				Merke: Jedes Objekt, das auf weniger als die Armlänge des Roboters
				von ihm entfernt zu seiner Rechten aufgestellt ist, wird als Glas erkannt.
				*/
		
		
				if(found){ // found ist true, wenn ein Glas entdeckt wurde.
				  Serial.println("Glas gefunden");
			  driveStraight = true;
				  if(passed){ // wird ausgeführt, wenn ein Glas gefunden und vom Sonar passiert wurde
					 Serial.println("Glas passed");
					 glassDistanz = glassDistanz/glassCounts;
					 Serial.print("GlassDistanz = ");
					 Serial.println(glassDistanz);
					 double h = calcH();   //fahre restliche Distanz 
					 Serial.print("h = ");
					 Serial.println(h);
					 double steps = convertToSteps(h)-(length/2);
					 Serial.print("steps = ");
					 Serial.println(steps);
					 Glaserkennung();    //damit drive length nicht abändert
					 for(int i=0; i<steps; i++){
					   drive();
					 }
					 moveArm(glassDistanz+radiusGlas);   //Bewege Arm
					 fill();   //Füllen
					 resetArm();   //resetten     
					 length=0;           //length
					 glassDistanz = 0;  
					 glassCounts = 0;
			
				  }else{ // wird ausgeführt, wenn ein Glas gefunden aber noch nicht vollständig vom Glas passiert wurde
					for(int i = 0; i<5; i++)  drive();   //fahre und zähle Schritte
				  }
	
				}else{ // wird ausgeführt, wenn noch kein Glas gefunden wurde
				  driveStraight = false;
				  for(int i = 0; i<5; i++) drive(); //fahre bis zum nächsten Glas
				}
		
	
	
		  }
	
		  /*
			Diese Methode setzt uns passed und found.
			D.h. wir überprüfen, ob wir an einem Glas vorbei fahren, es passieren oder kein Glas in der Nähe ist.
			Außerdem wird hier die Entfernung zum Glas mitgeschrieben - glassDistanz wird später durch Mess-Anzahl dividiert.
		   */
		  void Glaserkennung(){
			long distance = distanceSonar();
			Serial.print("distance = ");
			Serial.println(distance);
			if(distance > 0) {
			  found = true;
			  glassDistanz = glassDistanz + distance;
			  glassCounts++;
			}                                //wir haben ein Glas gefunden
			else if(found &&!passed && distance == 0) passed = true;    //gerade am Glas vorbei                                     
			else if(found && passed && distance ==0){                  //wir haben das Glas bereits passiert=> auf Anfang setzen
			  passed = false; 
			  found = false;
			}
		  }
  	
  	
		  /*
			Methode, welche mit dem Sonar die Distanz zu etwaigen Objekten überprüft und uns in mm zurück gibt.
			Gibt es kein Objekt in direkter Nähe, wird 0 zurück gegeben, 
		   */
		  long distanceSonar(){
  	
			// Initialisiere Variablen für die Distanz 
			// duration sind ms und mm sind millimeter
			long duration = 0;
			long mm;
			for(int i=0; i<10;i++){
			  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
			  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
			  pinMode(pingPinSonar, OUTPUT);
			  digitalWrite(pingPinSonar, LOW);
			  delayMicroseconds(2);
			  digitalWrite(pingPinSonar, HIGH);
			  delayMicroseconds(10);
			  digitalWrite(pingPinSonar, LOW);
  	
			  // The same pin is used to read the signal from the PING))): a HIGH
			  // pulse whose duration is the time (in microseconds) from the sending
			  // of the ping to the reception of its echo off of an object.
			  pinMode(inPinSonar, INPUT);
			  duration = duration + pulseIn(inPinSonar, HIGH);
			  delay(25);
			 }
			// in mm umrechnen 
			mm = microsecondsToMillimeters(duration/10); 
	
			//TODO Schwelle erhöhen, falls möglich.
			if(mm > 160) mm = 0;
  	
			return mm;
		  }
  	
		  /*
			Methode, die den Arm in Ausgangsposition bringt.
		  */
		  void resetArm(){ 
			lift.write(100);  //TODO: nach oben zeigen lassen
			rotate.write(175);
		  }
	
		  /*
			Methode, welche die Armbewegung ausführt, d.h. ausfahren und senken.
		  */
		  void moveArm(double distance){
		  // TODO die Servos machen keine vollen 180° - das müssen wir manuell messen
		  // TODO hier müssen wir noch die Distanz des Sonar-Sensors auf die Achse des Rotators normieren
	
			double alpha = asin(distance/armLength); // Berechnung der Arm-Position bei gegebenem Abstand
    
			alpha = (alpha/pi)*180; // Umrechnung von Rad in Deg    
	
			delay(500L);
			rotate.write( (int) 175-alpha);    //Drehung
			delay(500L);
			lift.write(40); //TODO: Testen ob das gut ist //Senkung
		  }
  	
		  /*
			Restlänge, die Der Roboter zurücklegen muss, nachdem das Glas vom Sonar passiert wurde, aber ehe der Arm ausgefahren wird.
			TODO: Wird hier beachtet, wie viele Schritte wir bereits mit dem Sonar am Glas vorbeigefahren sind? -Rene
		  */
		  double calcH(){
			double g = glassDistanz + radiusGlas; //Distanz von Roboter zu Glasmitte
			Serial.print("g = ");
			Serial.println(g);
			double square = round(sqrt(sq((double) armLength) - sq(g))); // Wir casten hier auf Double, weil Arduino behindert ist.
			Serial.print("sqrt(armLength*armLength - g*g) = ");
			Serial.println(square);
			return distSonar - square; 
	
		  }
  	
		  /*
			Methode, die anhand der Elektroden am Zapfarm erkennt, ob das darunter befindliche Glas voll ist.
			Gibt bei Kontakt der Elektroden mit der Flüssigkeit (also Glas voll) true aus, sonst false.
		  */
		  boolean fuellstand(){
			 //Wenn Strom fließt, ist die Flüssigkeit hoch genug, um Strom zu leiten
			if(analogRead(fuellstandsPin) <= threshold) return false; 
			else return true;
	
		  }
  	
  
		  /*
			Öffnet das Ventil, wartet bis das darunter befindliche Glas voll ist, schließt das Ventil wieder.
		  */
		  void fill(){
			ventil.write(valveOpen);
			while(!fuellstand())  delay(100);
			ventil.write(valveShut);
		  }
	
  	
		  /*
			Methode Um MicroSekunden in mm umzurechnen. 
			Schallgeschwindigkeit liegt bei 340 m/s oder 29 mikrosekunden/cm. 
			Der Ping geht den Weg zum Objekt und zurück, also rechnen wir durch 2.
		  */
		  long microsecondsToMillimeters(long microseconds){
			 return (microseconds / 2.9 / 2);
		  }
	
	
		  /*
			Methode um an der Linie entlang zu fahren, Achtung: ändert die Variable length.
			rightWasLast und leftWasLast bleiben auch zwischen Iterationen der Methode erhalten,
			daher kann der Roboter auch nachdem die Linie überhaupt nicht mehr gesehen wird zu ihr
			zurückfinden.
			Der boolean driveStraight wird im Loop auf true gesetzt, wenn wir zu Messzwecken oder um ein
			Glas zu erreichen nur genau geradeaus fahren dürfen. Danach findet der Roboter trotzdem zur 
			Linie zurück.
			Mit dem int stepDelay lässt sich die Geschwindigkeit des Roboters regulieren.
		  */
     	 
        	  void drive(){
     	
			 brightnessLeft = (analogRead(photoL) + analogRead(photoL))/2;
			 brightnessRight = (analogRead(photoR) + analogRead(photoR))/2 + photoCorrection;
	
  	
			 //Überprüfe mit den Phototransitoren, wo die Linie ist 
			 //z.B. wenn Links mehr weiß ist als rechts, dann müssen wir von rechts über die Linie gefahren sein
			 if(brightnessLeft> brightnessRight + lightThreshold){  
				rightWasLast = true;
				leftWasLast = false;
			  } 
			  else if (brightnessRight > brightnessLeft + lightThreshold ){
				leftWasLast = true;
				rightWasLast = false;
			  } 
			  else if(brightnessRight < lightBlack && brightnessLeft < lightBlack){
				leftWasLast = false;
				rightWasLast = false;
			  }
	  
			  //Fahrbewegung
			  if(rightWasLast && !driveStraight){   //nach rechts fahren
				digitalWrite(stpL, HIGH); 
				delay(1);               
				digitalWrite(stpL, LOW);  
				delay(stepDelay);
			  }
			  else if(leftWasLast && !driveStraight){   //nach links fahren
				digitalWrite(stpR, HIGH);   
				delay(1);               
				digitalWrite(stpR, LOW);  
				delay(stepDelay);
			  }
			  else if((!rightWasLast && !leftWasLast) || driveStraight){   // geradeaus fahren
				digitalWrite(stpL, HIGH);
				digitalWrite(stpR, HIGH);   
				delay(1);
				digitalWrite(stpL, LOW);                 
				digitalWrite(stpR, LOW);  
				delay(stepDelay);
			  } 
	
	
			  //Wenn wir gerade an einem Glas vorbei fahren, zählen wir die Schritte
			 if(found && !passed){ //Merke: muss danach auf 0 gesetzt werden
				length++;
			  }
			}
	
			/*
			  Rechnet Stepperschritte in mm um
			*/
			long convertToMM(long steps){
			  return steps*stepLength;    
			}
	
			/*
			  Rechnet mm in Stepperschritte um
			*/
			long convertToSteps(long mm){
			  return mm/stepLength;
			}