//VARIABILI PER DEFINIRE LO STATO DEL SISTEMA

//i vari colori che possono assumere le sfere
t.colori = ["vuoto", "blu", "rosso", "altro"];
//cosa si trova in ogni posizione
t.stato = ["null", "null", "null", "null"];
//in che posizione si trova il braccio
t.posizione = 0;
//ho la palla tra le pinze?
t.palla = false;

//ABBASSA IL BRACCIO SU UN PIEDISTALLO
function scendi() {
	if (t.palla) { //ho la sfera: abbasso ad altezza giusta per posarla
		wheelR = -40;
		waituntil (sonar < 13 ~ 1s);
		wheelR = 0
	} else { //non ho la sfera: mi abbasso fino a toccarla o fino a quando mi accorgo che la posizione è vuota
		wheelR = -40;
		waituntil ((bumper == 1 || sonar < 10) ~ 1s);
		wheelR = 0
	};
	return
};

//SOLLEVO IL BRACCIO PER POTERLO RUOTARE
function sali() {
	wheelR = 40;
	waituntil (sonar > 14 ~ 500ms); //controllo l'altezza per fermermi
	wheelR = 0;
	return
};

//SE SENTO UNA SFERA LA PRENDO TRA LE PINZE
function chiudi() {
	if (bumper == 1 && !t.palla) { //sento che ci sia una palla tra le pinze e le pinze non siano già chiuse
		wheelR = 40; //stacco il sensore dalla palla
		waituntil (bumper == 0 ~ 1s);
		wait (3s);
		wheelR = 0;
		claw = 7; //chiudo le pinze
		waituntil (bumper == 1 ~ 500ms);
		claw = 0 time:2s;
		t.palla = true //aggiorno lo stato
	};
	return
};

//APRE LE PINZE
function apri() {
	claw = -3;
	wait (2s);
	claw = 0;
	t.palla = false; //non ho più la palla
	return
};

//RUOTO DI UNA POSIZIONE NEL SENSO INDICATO DAL PARAMETRO (true ORARIO, false ANTIORARIO)
function gira(orario) {
	if (orario) {
		if (t.posizione < 3) { //controllo di non andare fuori dai limiti
			wheelL = -60;
			wait (8s);
			t.posizione++ //aggiorno lo stato
		} else {
			echo "!!!Fondo corsa!!! (1)"
		}
	}
	else {
		if (t.posizione > 0) {
			wheelL = 60;
			wait (9s); //il motore è leggermente più lento in questo senso
			t.posizione--
		} else {
			echo "!!!Fondo corsa!!! (0)"
		}
	};
	wheelL = 0;
	return
};

//SE IL BRACCIO + ABBASSATO RESTITUISCE IL CONTENUTO DELLA POSIZIONE CORRENTE, ALTRIMENTI vuoto
function guarda() {
	if (bumper == 1) { //sono su una sfera
		if (light > 0.20 && light < 0.44) return t.colori[1]; //quantità di luce riflessa dalla palla blu in condizioni di luce solare
		if (light > 0.45 && light < 0.58) return t.colori[2]; //idem per il rosso
		return t.colori[3] //qualunque altro colore esso sia
	} else //non sento il bumper: piedistallo vuoto
		return t.colori[0]
};

//RIPORTA IL BRACCIO IN UNA POSIZIONE NOTA, DETERMINABILE MEDIANTE SENSORI POI RITORNA IN POSIZIONE ZERO
function azzera() {
	sali(); //alzo il braccio
	wheelL = 75; //vado a fondo-corsa
	waituntil (sonar < 8 ~ 1s);
	wheelL = 0;
	wait (1s);
	wheelL = -60; //ritorno in posizione 0
	wait (6s500ms);
	wheelL = 0;
	t.posizione = 0; //aggiorno lo stato
	return
};

//PORTA IL BRACCIO ALLA POSIZIONE INDICATA DAL PARAMETRO
function vai(pos) {
	if (pos != t.posizione) { //devo spostarmi o ci sono già?
		sposta = pos - t.posizione; //di quanto mi sposto
		if (sposta < 0) { //in che direzione mi sposto
			sposta = -sposta;
			senso = false
		} else {
			senso = true
		};
		loopn(sposta) { //eseguo un numero di spostamenti necessari
			gira(senso); //nel senso appena determinato
			wait(1s) //con pause tra uno spostamento e l'altro per migliorare la precisione della posizione
		}
	};
	return
};

//RESTITUISCE IN CHE POSIZIONE SI TROVA IL COLORE PASSATO COME PARAMETRO
function dove(colore) {
	for (i = 0; i < 3; i++) { //non uso sensori, guardo lo stato interno del sistema
		col = t.stato[i]; //controllo le prime 3 posizioni (da 0 a 2)
		if (col == colore)
			return i
	};
	return 3 //altrimenti è l'ultima per forza!
};

//PRENDE LA PALLA DEL COLORE INDICATO DAL PARAMETRO E LA POSIZIONA SULLO SPAZIO VUOTO
function sposta(colore) {
	palla = dove(colore); //trovo la posizione della palla da spostare
	buco = dove(t.colori[0]); //e dello spazio in cui metterla
	vai(palla); //prendo la palla
	scendi();
	chiudi();
	sali();
	vai(buco); //la metto al posto vuoto
	scendi();
	apri();
	sali();
	t.stato = modifica(t.stato, buco, colore); //aggiorno lo stato
	t.stato = modifica(t.stato, palla, t.colori[0]);
	return
};

//RICEVE IN INGRESSO UN ARRAY, UN INTERO E UN VALORE QUALSIASI, RESTITUISCE L'ARRAY D'INGRESSO CON IL VALORE NELLA POSIZIONE PASSATA COME PARAMETRO SOSTITUITO DAL TERZO PARAMETRO
function modifica(array, pos, val) { //O(n)
	i = 0;
	buf = [];
	foreach n in array {
		if (pos == i)
			buf = buf + val
		else
			buf = buf + n;
		i++
	};
	return buf
};

//TOLGO DA UN ARRAY TUTTI GLI ELEMENTI DEL VALORE PASSATO COME PARAMETRO
function togli(array, val) { //O(n)
	i = 0;
	buf = [];
	foreach n in array {
		if (n != val)
			buf = buf + n
		};
	return buf
};

//RESTITUISCE IL NUMERO DI ELEMENTI IN UN ARRAY -- sostituita da dim array (t.ultimo +1)
function conta(vet) {
	k = 0;
	foreach n in vet {
		k++
	};
	return k
};
//
//DATO UN ARRAY NE RESTITUISCE UNO CON LE POSIZIONI IN ORDINE INVERSO
function inverti(vett) { //O(n)
	if (tail(vett) == []) { //vettore vuoto o con un elemento solo
		tmp = vett //non devo fare nulla
	} else {
		tmp = [];
		ult = conta(vett);
		ult--;
		for (i = ult ; i >= 0 ; i--) {
			el = vett[i];
			tmp = tmp + el
		}
	};
	return tmp
};

//TOGLIE L'ULTIMO ELEMENTO DA UN ARRAY
function togliultimo(v) { //O(n)
	l = t.ultimo + 1; // dimensione degli array Š sempre uguale a t.ultimo + 1
	if (l > 0) {
		k = 0;
		buf = [];
		foreach n in v {
			k = k + 1;
			if (k != l)
				buf = buf + n
		};
		return buf
	}
	else {
		return
	}
};

//CALCOLA IL VALORE DELL'EURISTICA PER UNO STATO IN FRONTIERA
function f(nodo) { //O(1)
	distanza = h(nodo);
	costo = g(nodo);              //la distanza è positiva (maggiore è più sono lontano dall'obiettivo)
	euristica = distanza + costo; //il costo è anche positivo, più è alto più mosse ho fatto per raggiungere questo nodo
	return euristica              //alla fine dovrò minimizzare l'euristica
};

//CALCOLA IL COSTO DI UN NODO COME COSTO DEL PADRE + 1
function g(n) { //O(1)
	if (n == 0) { //la radice ha costo 0 per definizione
		costo = 0
	} else {
		padre = t.padre[n]; // trovo il costo del padre
		costo = t.pesi[padre];
		costo = costo + 1 //il costo del nodo è pari al costo del padre + 2
	};
	return costo
};

//STIMA LA DISTANZA DAL NODO CORRENTE AL NODO OBIETTIVO
function h(n) { //O(1)
	att = t.stati[n]; //simulo la successione di mosse per raggiungere quello stato
	valore = 0; //inizializzo il valore dell'euristica
	buco = t.colori[0];
	for (i = 0; i <= 3; i++) { //confronto ogni posizione dello stato simulato con lo stato obiettivo
		st = t.goal[i];
		a = att[i];
		if (st == a) { 
			if (st == buco) { //se lo spazio è in posizione giusta ho uno svantaggio perché mi serve una mossa in più
				valore = valore + 0.5 //quindi incremento di uno
			}
		}
		else {
			if (st != buco) { //per ogni palla in posizione sbagliata incremento di 2 il valore dell'euristica
				valore = valore + 1
			}
		}
	};
	return valore
};

//DATO UNO STATO IN FRONTIERA RESTITUISCE LE MOSSE NECESSARIE PER RAGGIUNGERLO PARTENDO DALLO STATO INIZIALE
function elencomosse(n) { //O(n)
	m = []; //vettore che contiene le mosse
	papa = t.padre[n]; //procedo a ritroso visitando il padre del nodo
	while (papa != "null") { //finché non raggiungo la radice dell'albero
		m = m + t.mossa[n]; //mi salvo le mosse che ho compiuto ogni volta
		n = t.padre[n];
		papa = t.padre[n]
	};
	m = inverti(m); //mi ritrovo il vettore al contrario, devo invertire ogni posizione
	return m
};

//DATO UNO STATO ED UN COLORE SCAMBIA LA PALLA DI QUEL COLORE CON LA POSIZIONE VUOTA
function scambia(st, colore) { //O(1)
	for (i = 0; i <= 3; i++) { //trovo la posizione di quel colore
		col = st[i];
		if (col == colore)
			palla = i
	};
	for (i = 0; i <= 3; i++) { //trovo la posizione dello spazio vuoto
		col = st[i];
		if (col == t.colori[0])
			buco = i
	};

	st = modifica(st, buco, colore); //aggiorno lo stato
	st = modifica(st, palla, t.colori[0]);
	return st
};

//TROVO L'ELEMENTO IN FRONTIERA CON IL VALORE DI EURISTICA MINIMO
//(IN CASO DI PARITA' IL PRIMO ELEMENTO CON TALE EURISTICA)
function eurmin(front) { //O(n)
	min = "null"; //inizializzo il massino
	foreach n in front { //scorro tutti gli elementi in frontiera
		eur = t.euristica[n];
		if (min == "null" || eur < min) {
			min = eur;
			elem = n
		}
	};
	return elem
};

//DATO UNO STATO IN FRONTIERA DICE SE ESISTE UNO STATO PADRE CON LA STESSA CONFIGURAZIONE
function stessopadre(n) { //O(log(n))
	st = t.stati[n];
	papa = t.padre[n]; //procedo a ritroso visitando il padre del nodo
	while (papa != "null" && st != t.stati[papa]) { //finché non raggiungo la radice dell'albero o ho trovato un padre uguale
		papa = t.padre[papa]
	};
	if (st == t.stati[papa]) //ho trovato un padre uguale
		return true
	else
		return false
};