/* ***********************************************************************
*       Program: find.nxc
*       Author: Lukas Probst
*		  Email: lukas@internet-freaks.ner
*		  Homepage: http://lukas.internet-freaks.net
*
*
*
*************************************************************************/

/************************
*	INCLUDES					*
************************/

#include "NXCDefs.h"

/************************
*	DEFINES					*
************************/

// states (of the main task)
#define SUCHEN    0
#define HIN       1
#define GREIF     3
#define ASK       4
#define BACK      5
#define DA        6		
#define STOP      7
// other
#define GRENZE_HIN 50
#define GRENZE_GREIF 8
#define MAX_STEPS 1000
#define ASKTIME 5000
#define TURNTIME 40
#define CLAP 45
#define RED 111
#define BLUE 222

/************************
*	VARIABLES				*
************************/

int state;
int ultra, light, sound;
long step[] = {};
long i=0;
long zahl=0;
long endtime;
int ball_in, ball;
int dir;
int get_red, get_blue;

/************************
*	ERKENNUNGS FUNCTIONS *
************************/

sub get_values() { // setzt Helligkeitswerte
	TextOut(0,LCD_LINE1,"Roter Ball?");
	while(Sensor(IN_4)==0) { // solange nicht geklickt
		TextOut(0,LCD_LINE2,"Warte...");
		get_red = Sensor(IN_2);
	}
	#define RED_BALL get_red
	ClearScreen();
	Wait(1000); // das klick nicht 2mal gilt
	
	TextOut(0,LCD_LINE1,"Blauer Ball?");
	while(Sensor(IN_4)==0) {	// solange nicht geklickt
		TextOut(0,LCD_LINE2,"Warte...");
		get_blue = Sensor(IN_2);
	}
	#define BLUE_BALL get_blue
	ClearScreen();
	
	while(sound < CLAP) { // warte auf klatschen
		sound = Sensor(IN_3); // speichert Wert des Geräuschsensors in "sound" 
	}
	ClearScreen();
}

/************************
*	OUTPUT FUNCTIONS		*
************************/
int ball_red() { //erkennt den roten Ball und gibt true zurück
	if(light >= RED_BALL-3 && light <= RED_BALL+3) {
		return 1;
	}
	else {
	   return 0;
	}
}

int ball_blue() { //erkennt den blauen Ball und gibt true zurück
	if(light >= BLUE_BALL-3 && light <= BLUE_BALL+3) {
		return 1;
	}
	else {
	   return 0;
	}
}

/************************
*	SUB FUNCTIONS			*
************************/

sub define_ball() { // speichert die Farbe des Balles in der Variable "ball"
	if(ball_red()==1) {
		ball=RED;
	}
	else if(ball_blue()==1) {
		ball=BLUE;
	}
}

sub auf() { // öffnet die Zange des Roboters
	RotateMotor(OUT_C, 30, 80);
	ball_in = 0;
	ClearScreen();
}

sub zu() { // schließt die Zange des Roboters
	RotateMotor(OUT_C, 30, -80);
	ball_in = 1;
	ClearScreen();
}

sub beep() { // macht Beep
	PlayToneEx(440,1000,4,0); //erzeugt für 1 Sekunde einen Kammerton A in maximaler Lautstaerke und wird nicht wiederholt 
	Wait(1000);	// wartet auf Ende des Tones	
}

sub get_ultra() { // hohlt den Wert des Ultraschallsensors
	Wait(50); // dass große werte ankommen
	ultra = SensorUS(IN_1); // und speichert sie in der Variablen "ultra"
}

sub display() { // zeigt Werte auf dem Display 
	TextOut(0,LCD_LINE1,"Ultra");
	NumOut(60,LCD_LINE1,ultra);
	TextOut(0,LCD_LINE2,"Light");
	NumOut(60,LCD_LINE2,light);
	LineOut (0, LCD_LINE3, 100, LCD_LINE3);
	if(ball_in==1) {	// wenn ball da
		CircleOut(20, LCD_LINE6, 15);
		if(ball==RED) {	TextOut(60, LCD_LINE6,"red_"); }
		else if(ball==BLUE) { TextOut(60, LCD_LINE6,"blue"); }
	}
}
/************************
*	STATE FUNCTIONS		*
************************/

sub suchen() { // sucht den Ball
	if(dir==0) { // gegen Uhrzeigersinn
		OnRev(OUT_A, 20);
		OnFwd(OUT_B, 20);
	}
	else if(dir==1) {	// mit Uhrzeigersinn
		OnFwd(OUT_A, 20);
		OnRev(OUT_B, 20);
	}
	if(ultra <= GRENZE_HIN) { // wenn näher als 50
		Wait(20);
		get_ultra(); // erneutes prüfen
		if(ultra <= GRENZE_HIN) {		
			state = HIN; // Statuswechsel zu HIN
			ResetAllTachoCounts(OUT_AB); // setzt Drehsensor zurück
			zahl++; // nächste Spalte im Array
		}
	}
}

sub hin() { // fährt zum Ball
	OnFwdReg(OUT_AB, 30, 1); // synchrones Fahren (100% grade)
	if(ultra > GRENZE_HIN) { // wenn nichts mehr sichtbar
		Wait(20);
		get_ultra(); // erneutes prüfen
		if(ultra > GRENZE_HIN) {	
			state = SUCHEN; // Statuswechsel zu SUCHEN
			ResetAllTachoCounts(OUT_AB); // setzt Drehsensor zurück
			zahl++; // nächste Spalte im Array
		}
	}
	else if(ultra <= GRENZE_GREIF) { // wenn direkt vorm Ball
		state = GREIF; // Statuswechsel zu GREIF
		ResetAllTachoCounts(OUT_AB); // setzt Drehsensor zurück
		zahl++; // nächste Spalte im Array
	}
}

sub greif() { // greift den Ball
	Off(OUT_AB); // bewegt sich nicht mehr
	zu(); // Zange zu
	beep(); // Beep
	endtime = CurrentTick()+ASKTIME; // beginn der 5 Sekunden Wartezeit
	light = Sensor(IN_2); // hohlt die Farbe des Balles
	define_ball(); // und speichert sie in "ball"
	state = ASK; // Statuswechsel zu ASK
}

sub ask() { // frägt ob Ball erwünscht
	if(CurrentTick() <= endtime) { // wenn die Zeit noch läuft
		if(sound >= CLAP) { // wenn geklatscht
		   state = BACK; // Statuswechsel zu BACK
		}
	}
	else { // wenn die Zeit vorbei ist
		auf(); // Zange auf
		beep(); // BEEP
		state = BACK; // Statuswechsel zu BACK
	}
}

sub back() { // zurück zum Ausgangspunkt
	i = zahl;
	repeat(zahl+1) { // soviele Schritte wie es gibt
		if(step[i]<0) { // wenn gedreht
			if(dir==0) { // wenn gegen urzeigersinn
				RotateMotorEx(OUT_AB, 30, step[i], -100, true); // dann mit drehen
			}
			else if(dir==1) { // wenn mit urzeigersinn
				RotateMotorEx(OUT_AB, 30, step[i], 100, true); // dann gegen drehen
			}
		}
		else { // wenn gerade
			RotateMotorEx(OUT_AB, 30, -step[i], 0, true); // grade aus fahren
		}
		i--;
	}
	ResetAllTachoCounts(OUT_AB); // setzt Drehsensor zurück
	i = 0; // zurücksetzen
	zahl = 0; // zurücksetzen
   ArrayInit(step, 0, MAX_STEPS); // erstell Array neues Array für nächsten Lauf
	state = DA;	// Statuswechsel zu DA
}

sub da() { // Angekommen
	if(ball_in == 1) { // wenn Ball dabei
		state=STOP; // Statuswechsel zu STOP
		ClearScreen();
	}
	else { // wenn kein Ball dabei
		// Richtungswechsel:		
		if(dir==0) {
			dir = 1; 
		}
		else if(dir==1) {
			dir = 0;
		}
		state = SUCHEN; // Statuswechsel zu SUCHEN
	}
	beep(); // BEEP
}

/************************
*	MAIN TASK      		*
************************/

task main() { 
   ArrayInit(step, 0, MAX_STEPS); // erstell Array zur Schritterfassung
	FirstTick(); // beginnt Zeit
	SetSensorLowspeed(IN_1); // definiert Port 1 als Ultraschallsensor
	SetSensorLight(IN_2); // definiert Port 2 als Lichtsensor
	SetSensorSound(IN_3); // definiert Port 3 als Geräuschsensor
	SetSensorTouch(IN_4); // definiert Port 4 als Tastsensor
	get_values(); // setzt die Helligkeitswerte (Defines)
	ResetAllTachoCounts(OUT_ABC); // setzt Drehsensor zurück
	while(true) { // solange Programm läuft
		Stop(zahl >= MAX_STEPS); // wenn Array zu klein --> beenden (sollte nicht passieren)
		Stop(state == STOP);	// wenn Programm zueende --> beenden
		if(dir==0) {
			step[zahl] = MotorRotationCount(OUT_A); // Speichert Rotation von Motor A in Array
		}
		else if(dir==1) {
			step[zahl] = MotorRotationCount(OUT_B); // Speichert Rotation von Motor B in Array
		}		
		get_ultra(); // speichert Wert des Ultraschallsensors in "ultra"
		light = Sensor(IN_2); // speichert Wert des Lichtsensors in "light"
		sound = Sensor(IN_3); // speichert Wert des Geräuschsensors in "sound"
		switch(state) { // startet dem Status entsprechendes Programm
			case SUCHEN: suchen(); break;
			case HIN: hin(); break;
			case GREIF: greif(); break;
			case ASK: ask(); break;
			case BACK: back(); break;			
			case DA: da(); break;
		}
		display(); // zeigt Werte auf dem Display
	}
}