//genutzte headerfiles definieren
#include <conio.h>			
#include <unistd.h>
#include <dmotor.h>
#include <dsensor.h>

							//sensoren definieren 
							//sensor 1 und sensor 2 sind taster
							//sensor 3 ist lichtsensor
#define TOUCH_1 TOUCH(SENSOR_1)
#define TOUCH_2 TOUCH(SENSOR_2)
#define LIGHT_3 LIGHT(SENSOR_3) 

int up_down_time=0;			//zur vertikalen Positionsbestimmung

int actual_round=0;			//Rundenzähler
int initcount;		
int max_bright=0;			//Speicher für maximale Helligkeit
int max_bright_ever;
long time_bright=0;
long bright_pos=0;
long sys_up_time_old=0;	//da Systemuhr 32-bit Zähler => Startzeit festhalten nötig
long long_light;			
void get_old_time()			//zum merken der aktuell hellsten Position
{
	sys_up_time_old=get_system_up_time();
}
void init() 
{
	
				//Motoren erstmal aus, sonst aber immer mit voller Leistung betreiben
	motor_a_dir(off);	//Motor_a = vertikal
	motor_b_dir(off);	//Motor_b = horizontal
	motor_c_dir(off);	//Motor_c = Blendenrelais fwd => Relais 1 schließt, Relais 2 öffnet 
				//rev => Relais 2 schließt, Relais 1 öffnet
	motor_a_speed(0xff);	// immer volle Leistung
	motor_b_speed(0xff);
	motor_c_speed(0xff);
	
	
	motor_a_dir(fwd);	//in Startposition fahren, vertikal bis Anschlag
	while (TOUCH_2==0) {}	//fahren, Motor bremsen => Position halten
	motor_a_dir(brake);
	
				//vertikale Anschlag/Anschlag Zeit bestimmen
							
	get_old_time(); 	//Systemzeit festhalten
	
	motor_a_dir(rev);	//vertikale Achse bis oberer Anschlag abfahren
	while (TOUCH_1==0) {}
	motor_a_dir(brake);	//Motor bremsen => Position halten

	up_down_time=get_system_up_time()-sys_up_time_old; 
				// Anschlag/Anschlag Zeit aus Systemzeit bestimmen
	cputw(up_down_time);	//Ausgabe zur Kontrolle auf dem RCX-Display		
}

void vertical_step()
{
	get_old_time();
	
	motor_a_dir(fwd); 	//vertikal um 1/4 des Maximalausschlags nach unten fahren
	while ((get_system_up_time()< (sys_up_time_old+up_down_time / 4)) && (TOUCH_2==0)) 
	{
		cputw(get_system_up_time);
	}
	motor_a_dir(brake); 	//Motor bremsen => Position halten
}

void horizontal_round()
{
	get_old_time();
	motor_b_dir(fwd);
	while (get_system_up_time()< (sys_up_time_old+0x1A00)) 
	{
		cputw(max_bright);
		if (LIGHT_3 >= max_bright) 
		{
			max_bright=LIGHT_3;
			time_bright=get_system_up_time();
		}
	}
	motor_b_dir(brake);
	bright_pos=get_system_up_time()-time_bright;		
}

void back2spot()
{
	get_old_time();
	motor_b_dir(rev);

	while (get_system_up_time()< (sys_up_time_old+bright_pos)) 
	{
		cputw(max_bright);
	}
	motor_b_dir(brake);	
}

/***************************************************************************************
 *                                 Hauptroutine					       *
 ***************************************************************************************/

int main(int argc, char **argv) 
{  
	int count;
	init();
	motor_c_dir(fwd);
	while ((max_bright<0x4A) && (TOUCH_2==0))
	{
		vertical_step();
		horizontal_round();
	}

	back2spot();
	motor_c_dir(rev);

	motor_a_dir(fwd);	//in Startposition fahren, vertikal bis Anschlag
	while (TOUCH_2==0) {}	//fahren, Motor bremsen => Position halten
	motor_a_dir(brake);
	motor_a_dir(rev);
	max_bright=0;
	while (((LIGHT_3 < 0x50)||(LIGHT_3 >= max_bright)) && (TOUCH_1 ==0))
	{
		max_bright=LIGHT_3;
		cputw(max_bright);
	}
	motor_a_dir(brake);
	max_bright_ever=max_bright;
	while (!shutdown_requested()) 
	{	
		if (LIGHT_3<max_bright_ever-10) 
		{
			motor_c_dir(fwd);
			get_old_time();
			motor_b_dir(fwd);
			while (get_system_up_time()<=sys_up_time_old+0x0200) {}	

			motor_b_dir(rev);
			max_bright=0;
			while (((LIGHT_3 < 0x50)||(LIGHT_3 >= max_bright)) && (TOUCH_1 ==0))
			{
				max_bright=LIGHT_3;
			}
			motor_b_dir(brake);	
				// steht in Richtung Sonne, wenn alles geklappt hat
		
			motor_c_dir(rev);

			get_old_time();
			motor_a_dir(fwd);
			while (get_system_up_time()<=sys_up_time_old+0x200) {}	

			motor_a_dir(rev);
			max_bright=0;
			while (((LIGHT_3 < 0x50)||(LIGHT_3 >= max_bright)) && (TOUCH_1 ==0))
			{
				max_bright=LIGHT_3;
			}
			motor_a_dir(brake);	
		}
		get_old_time();
		while (get_system_up_time()<=sys_up_time_old+0x1000)
		{
			long_light=LIGHT_3;
			cputw(long_light<<8 | ((get_system_up_time()-sys_up_time_old)>>4) );
	
		}	
		
	}
  	return 0;
}