#include "avr.h"
#include "global.h"
#include "hardware.h"
#include "util.h"
#include "LineSensor.h"
#include "LineSensorArray.h"
#include "Motor.h"
#include "Navigation.h"
#include "Srf10.h"











static void delay() {
	for(unsigned long i = 0; i < 25000; i++)
		asm("nop");
}


int main() {
	Status status = Ok;
	
	initHardware();
	
	Motor *motorLeft = new Motor(&PORTD, &OCR1BL, 0x01, 0x02);
	Motor *motorBack = new Motor(&PORTD, &OCR1AL, 0x04, 0x08);
	Motor *motorRight = new Motor(&PORTB, &OCR0, 0x01, 0x02);
	
	Navigation *navigation = new Navigation(motorLeft, 60.0, motorBack, 180.0, motorRight, 300.0);
	
	LineSensor *lineSensors[5];
	
	for(int i = 0; i < 5; i++)
		lineSensors[i] = new LineSensor(i);
	
	LineSensorArray *lineSensorArray = new LineSensorArray(lineSensors);
	
	lineSensorArray->setMinimumDifference(CAL_MIN_DIFF);
	lineSensorArray->setMaximumWhiteDifference(CAL_MAX_WHITE_DIFF);
	
	//Srf10 *srf10Right = new Srf10(0xE0);
	Srf10 *srf10Left = new Srf10(0xE2);
	
	//srf10Right->setUnit(Centimeters);
	srf10Left->setUnit(Centimeters);
	
	//Srf10 *srf10Last = srf10Left;
	
	
	do {
		lineSensorArray->update();
	} while(!lineSensorArray->calibrate());
	
	navigation->setSpeed(DEFAULT_SPEED);
	
	while(true) {
		lineSensorArray->update();
		
		//srf10Last = (srf10Last==srf10Left)?srf10Right:srf10Left;
		//srf10Last->updateDistance();
		
		switch(status) {
			case Ok:
				if(lineSensorArray->getLine() == 0.0 && lineSensorArray->getSharpness() == 1.0)
					lineSensorArray->calibrate();
				
				// TODO: victims!!
				
				// TODO: debris!!
				
				if(lineSensorArray->getSharpness() < 0.0)
					break;
				else if(lineSensorArray->getSharpness() == 0.0) {
					// TODO: white?
				}
				else {
					if(lineSensorArray->getLine() == 0.0) {
						navigation->setSpeed(DEFAULT_SPEED);
						navigation->setDirection(0.0);
						navigation->setSpin(0.0);
					}
					else if(ABS(lineSensorArray->getLine()) < 1.0) {
						navigation->setSpeed(DEFAULT_SPEED);
						navigation->setDirection(0.0);
						
						if(lineSensorArray->getLine() < 0.0)
							navigation->setSpin(CURVE_SPIN);
						else
							navigation->setSpin(-CURVE_SPIN);
					}
					else if(ABS(lineSensorArray->getLine()) < 1.5) {
						navigation->setSpeed(DEFAULT_SPEED);
						
						if(lineSensorArray->getLine() < 0.0) {
							navigation->setDirection(STRAFE_DIRECTION);
							navigation->setSpin(CURVE_SPIN);
						}
						else {
							navigation->setDirection(-STRAFE_DIRECTION);
							navigation->setSpin(-CURVE_SPIN);
						}
					}
					else {
						navigation->setSpeed(0.0);
						
						if(lineSensorArray->getLine() < 0.0)
							navigation->setSpin(TURN_SPIN);
						else
							navigation->setSpin(-TURN_SPIN);
					}
				}
					
				break;
			case White:
				break;
			case Debris:
				break;
		}
	}
	
	return 0;
}