#include <stdlib.h>
#include <time.h>

#include "agents.h"


int agent_target = -1;
float odleglosc = 10000000.0;
float odlegloscNowa = 0.0;
int index = -1;
int typ = 0;

AutoPilot::AutoPilot()
{
}

void AutoPilot::AutoControl(MovableObject *obj)
{
	Terrain *_terrain = obj->terrain;

	float pi = 3.1415;
	//podnoszenie_przedm = 1;
	Vector3 vect_local_forward = obj->state.qOrient.rotate_vector(Vector3(1, 0, 0));
	Vector3 vect_local_right = obj->state.qOrient.rotate_vector(Vector3(0, 0, 1));
	// TUTAJ NALEŻY UMIEŚCIĆ ALGORYTM AUTONOMICZNEGO STEROWANIA POJAZDEM
	// .................................................................
	// .................................................................
	if (agent_target == -1) {
		odleglosc = 10000000.0;
		odlegloscNowa = 0.0;
		index = -1;
		for (int i = 0; i < _terrain->number_of_items; i++)
		{
			if (obj->state.amount_of_fuel < 150.0)
				typ = ITEM_BARREL;
			else
				typ = ITEM_COIN;
			if (_terrain->p[i].type == typ && _terrain->p[i].to_take)
			{
				odlegloscNowa = (_terrain->p[i].vPos - obj->state.vPos + Vector3(0, obj->state.vPos.y - _terrain->p[i].vPos.y, 0)).length();
				if (odleglosc > odlegloscNowa)
				{
					odleglosc = odlegloscNowa;
					index = i;
				}
			}
		}
		agent_target = index;
	}
	else if (agent_target >= 0 && _terrain->p[agent_target].to_take)
	{

		Vector3 t = _terrain->p[agent_target].vPos - obj->state.vPos;
		Vector3 w_przod = obj->state.qOrient.rotate_vector(Vector3(1, 0, 0));// obroc_wektor(Wektor3(1, 0, 0));

		float cos = (t^w_przod) / (t.length()*w_przod.length ());
		float kat = acos(cos);

		if (cos > 0.95)
		{
			obj->state.wheel_turn_angle= 0;
		}
		else
		{
			if (kat < PI)
				obj->state.wheel_turn_angle = -PI / 2 - .1f;
			else
				obj->state.wheel_turn_angle = +PI / 2 - .1f;
		}

		if (obj->state.vV.length() < 8.0){
			obj->F = 5000.0;
		}
		else
			obj->F = 0.0;

		if ((_terrain->p[agent_target].vPos - obj->state.vPos + Vector3(0, obj->state.vPos.y - _terrain->p[agent_target].vPos.y, 0)).length() < 5.0*obj->radius)
		{
			
			if (obj->state.vV.length() > 3.0)
				obj->F = -1500.0;

		}
	}
	else {
		agent_target = -1;
	}
}

void AutoPilot::ControlTest(MovableObject *_ob, float krok_czasowy, float czas_proby)
{
	bool koniec = false;
	float _czas = 0;               // czas liczony od początku testu
	//FILE *pl = fopen("test_sterowania.txt","w");
	while (!koniec)
	{
		_ob->Simulation(krok_czasowy);
		AutoControl(_ob);
		_czas += krok_czasowy;
		if (_czas >= czas_proby) koniec = true;
		//fprintf(pl,"czas %f, vPos[%f %f %f], got %d, pal %f, F %f, wheel_turn_angle %f, breaking_degree %fn",_czas,_ob->vPos.x,_ob->vPos.y,_ob->vPos.z,_ob->money,_ob->amount_of_fuel,_ob->F,_ob->wheel_turn_angle,_ob->breaking_degree);
	}
	//fclose(pl);
}

// losowanie liczby z rozkladu normalnego o zadanej sredniej i wariancji
float Randn(float srednia, float wariancja, long liczba_iter)
{
	//long liczba_iter = 10;  // im wiecej iteracji tym rozklad lepiej przyblizony
	float suma = 0;
	for (long i = 0; i < liczba_iter; i++)
		suma += (float)rand() / RAND_MAX;
	return (suma - (float)liczba_iter / 2)*sqrt(12 * wariancja / liczba_iter) + srednia;
}