#include "ball_tracker.h"

//-----------------------------------------------------------------------------
void Ball_Tracker::Update()
{
	uint8 sensorCount = (IO_SENSOR_IR_330_DEG - IO_SENSOR_IR_0_DEG) + 1;
	uint16 intensity[sensorCount];
	uint8 greatestIntensity = 0;
	for(uint8 i = 0; i < sensorCount; i++)
	{
		IR_Sensor* currentSensor = parent->GetModule<IR_Sensor>(i + IO_SENSOR_IR_0_DEG);

		intensity[i] = 1023 - currentSensor->GetIRIntensity();

		if(intensity[i] < 24)
		{
			intensity[i] = 0;
		}
		
		if(intensity[i] > intensity[greatestIntensity])
		{
			greatestIntensity = i;
		}

		if(i == 0)
		{
			if(intensity[i] > BALL_HELD_INTENSITY) // Ball derzeit sehr nah dran
			{
				ballHeld = true;
				ballHeldCounter = 100;
			}
			else if(ballHeldCounter > 0) // Oder vor kurzem erst sehr nah dran
			{
				ballHeld = true;
				ballHeldCounter--;		
			}
			else ballHeld = false; // ansonsten hat er den Ball nicht
		}
	}

	if(intensity[greatestIntensity])
	{
		uint8 secondIntensity = 0xFF;
		uint8 leftSensor = (greatestIntensity + 1) % sensorCount;
		uint8 rightSensor = (greatestIntensity + sensorCount - 1) % sensorCount;

		if(intensity[leftSensor])
		{
			secondIntensity = leftSensor;
		}

		if(intensity[rightSensor] > intensity[leftSensor])
		{
			secondIntensity = rightSensor;
		}

		float mainDirection;
		
		switch(greatestIntensity + IO_SENSOR_IR_0_DEG)
		{
			case IO_SENSOR_IR_0_DEG:
				mainDirection = 0;
				break;
			case IO_SENSOR_IR_30_DEG:
				mainDirection = 1.0f * PI / 6.0f;
				break;
			case IO_SENSOR_IR_60_DEG:
				mainDirection = 1.0f * PI / 3.0f;
				break;
			case IO_SENSOR_IR_100_DEG:
				mainDirection = 5.0f * PI / 9.0f;
				break;
			case IO_SENSOR_IR_180_DEG:
				mainDirection = PI;
				break;
			case IO_SENSOR_IR_260_DEG:
				mainDirection = 13.0f * PI / 9.0f;
				break;
			case IO_SENSOR_IR_300_DEG:
				mainDirection = 15.0f * PI / 9.0f;
				break;
			case IO_SENSOR_IR_330_DEG:
				mainDirection = 33.0f * PI / 18.0f;
				break;
			default:
				mainDirection = EMPTY_FLOAT;
				return;
				break;
		}

		if(secondIntensity != 0xFF)
		{
			float secondDirection;

			switch(secondIntensity + IO_SENSOR_IR_0_DEG)
			{
				case IO_SENSOR_IR_0_DEG:
					secondDirection = 0;
					break;
				case IO_SENSOR_IR_30_DEG:
					secondDirection = 1.0f * PI / 6.0f;
					break;
				case IO_SENSOR_IR_60_DEG:
					secondDirection = 1.0f * PI / 3.0f;
					break;
				case IO_SENSOR_IR_100_DEG:
					secondDirection = 5.0f * PI / 9.0f;
					break;
				case IO_SENSOR_IR_180_DEG:
					secondDirection = PI;
					break;
				case IO_SENSOR_IR_260_DEG:
					secondDirection = 13.0f * PI / 9.0f;
					break;
				case IO_SENSOR_IR_300_DEG:
					secondDirection = 15.0f * PI / 9.0f;
					break;
				case IO_SENSOR_IR_330_DEG:
					secondDirection = 33.0f * PI / 18.0f;
					break;
				default:
					secondDirection = EMPTY_FLOAT;
					return;
					break;
			}

			if(fabs(mainDirection - secondDirection) > PI)
			{
				min(mainDirection, secondDirection) += 2.0f * PI;
			}

			direction = (intensity[greatestIntensity] * mainDirection + 
									intensity[secondIntensity] * secondDirection) / 
									(intensity[greatestIntensity] + intensity[secondIntensity]);

			direction = easyAngle(direction);
		}
		else
		{
			direction = mainDirection;
		}
	}
	else
	{
		direction = EMPTY_FLOAT;
	}
}