#include "avr.h"
#include "global.h"
#include "hardware.h"
#include "timer.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);
	srf10Right->setGain(DISTANCE_GAIN);
	
	srf10Left->setUnit(Centimeters);
	srf10Left->setGain(DISTANCE_GAIN);
	
	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!!
				
				if(srf10Last->hasDistance() && srf10Last->getDistance() <= DISTANCE_RECOG) {
					navigation->setSpeed(DEFAULT_SPEED);
					navigation->setDirection(90.0);
					
					status = Debris;
					break;
				}
				
				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:
			case DebrisWhite:
				if(srf10Right->getDistance() < srf10Left->getDistance())
					navigation->setSpin(DISTANCE_SPIN);
				else if(srf10Right->getDistance() > srf10Left->getDistance())
					navigation->setSpin(-DISTANCE_SPIN);
				else
					navigation->setSpin(0);
				
				if((srf10Right->getDistance()+srf10Left->getDistance())/2 > DISTANCE_MAX)
					navigation->setDirection(90.0-DISTANCE_ANGLE);
				else if((srf10Right->getDistance()+srf10Left->getDistance())/2 < DISTANCE_MIN)
					navigation->setDirection(90.0+DISTANCE_ANGLE);
				else
					navigation->setDirection(90.0);
				
				if(status == Debris && lineSensorArray->isSensorWhite(2))
					status = DebrisWhite;
				else if(status == DebrisWhite && lineSensorArray->isSensorBlack(2)) {
					navigation->setSpeed(0.0);
					navigation->setSpin(TURN_SPIN);
					status = Turn;
				}
				break;
			case Turn:
				if(lineSensorArray->isSensorWhite(2))
					status = TurnWhite;
				break;
			case TurnWhite:
				if(lineSensorArray->isSensorBlack(2)) {
					navigation->setSpeed(DEFAULT_SPEED);
					navigation->setDirection(0.0);
					navigation->setSpin(0.0);
					
					status = Ok;
				}
		}
	}
	
	return 0;
}