#include "NXCDefs.h"

// Definiere Konstanten
#define BLACK 0
#define LEFT 1
#define RIGHT 2
#define GRENZE 35
#define GESCHW 25
#define TURNTIME 50

// Erstelle Variabeln
int state;
int light;
int endtime, currenttime;
int rem=1;
int turntime=TURNTIME;
int rota_A, rota_B, rota_B_cor, dif;

sub fturn() {
	turntime=turntime+turntime+TURNTIME; // vergrößert Drehzeit
}

sub black() {
	OnRev(OUT_AB, GESCHW); // fahren
	turntime=TURNTIME; // setzt Drehzeit zurück
	TextOut(50, 60, "BLACK"); // Zeigt Status auf dem Display
   if (light >= GRENZE) { // wenn Wert größer als Grenzwert
   	endtime = CurrentTick()+turntime; // setzt Endzeit
      state = rem; // wechsle Status auf LEFT
   }
}

sub left() {
	OnRev(OUT_B, GESCHW); // nach
	OnFwd(OUT_A, GESCHW); // links
	TextOut(50, 60, "LEFT"); // Zeigt Status auf dem Display
	if (light <= GRENZE) { // wenn Wert kleiner als Grenzwert
		rem = LEFT;
   	state = BLACK; // wechsle Status auf BLACK
   }
   else if (endtime <= currenttime) { // wenn die Drehzeit vorbei ist
   	fturn(); // erhöht Drehzeit
   	endtime = CurrentTick()+turntime; // setzt Endzeit
   	state = RIGHT; // wechsel auf RIGHT
   }
}

sub right() {
	OnRev(OUT_A, GESCHW); // nach
	OnFwd(OUT_B, GESCHW); // rechts
	TextOut(50, 60, "RIGHT"); // Zeigt Status auf dem Display
	if (light <= GRENZE) { // wenn Wert kleiner als Grenzwert
		rem = RIGHT;	
   	state = BLACK; // wechsle Status auf BLACK
   }
   else if (endtime <= currenttime) { // wenn die Drehzeit vorbei ist
   	fturn(); // erhöht Drehzeit
   	endtime = CurrentTick()+turntime; // setzt Endzeit
   	state = LEFT; // wechsel auf RIGHT
   }
}

sub display() {
	TextOut(0, 60, "Status:");
	TextOut(0, 40, "Sensorwert:");
	NumOut(70, 40, light);
	TextOut(0, 25, "TURN:");
	NumOut(50, 25, turntime);
	TextOut(0, 15, "Motor A:");
	NumOut(50, 15, rota_A);
	TextOut(0, 0, "Motor B:");
	NumOut(50, 0, rota_B);
}

task main() {  // Starte Programm
	FirstTick();
	SetSensorLight(IN_1); // Definiert IN_1 als Lichtsensor
	while(true) { // Solange nicht Beendet
		currenttime = CurrentTick(); // Speichert aktuelle Systemzeit in Variable
		light = Sensor(IN_1); // Liest den Sensor aus
		rota_A= MotorRotationCount(OUT_A);
		rota_B= MotorRotationCount(OUT_B);
		display();
		if (state == BLACK) { // wenn Status=BLACK
			black(); // Rufe Funktion black() auf
      }
    	else if (state == LEFT) { // wenn Status=LEFT
			left(); // Rufe Funktion left() auf
      }
      else if (state == RIGHT) { // wenn Status=RIGHT
			right(); // Rufe Funktion right() auf
      }
   ResetScreen();   
	}
}