#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(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 = -1.0f;
				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 = -1.0f;
					return;
					break;
			}

			direction = (intensity[greatestIntensity] * mainDirection + 
									intensity[secondIntensity] * secondDirection) / 
									(intensity[greatestIntensity] + intensity[secondIntensity]);
		}
		else
		{
			direction = mainDirection;
		}
	}
	else
	{
		direction = -1.0f;
	}
}
