//OBIETTIVO DA RAGGIUNGERE
t.goal = ["blu", "rosso", "altro", "vuoto"];
echo "Obiettivo: [" + t.goal[0] + " ; " + t.goal[1] + " ; " + t.goal[2] + " ; " + t.goal[3] + "]";
//OPERAZIONI PRELIMINARI
//load("funct.u");
apri();
azzera(); //reset del robot
//OSSERVO IL MONDO
loopn(3) {
	scendi(); //mi posiziono su un piedistallo
	colore = guarda(); //vedo se è presente una palla e di che colore è
	echo "Colore osservato: " +  colore + " (" + light + ")";
	t.stato = modifica(t.stato, t.posizione, colore); //aggiorno lo stato
	sali(); //passo alla posizione successiva
	if (t.posizione < 2)
		gira(true)
};
for (i = 0; i <= 3; i++) { //deduco cosa c'è in posizione 3 (l'ultima)
	colore = t.colori[i]; //per ogni colore
	pos = dove(colore);
	if (pos == 3) //vedo se è già stato assegnato ad una posizione
		t.stato = modifica(t.stato, 3, colore)
};
echo "Stato iniziale: [" + t.stato[0] + " ; " + t.stato[1] + " ; " + t.stato[2] + " ; " + t.stato[3] + "]";
//azzera(); //altro reset per migliorare la precisione
//PIANIFICO LE OPERAZIONI
temp = t.stato; //copia di t.stato per le simulazioni
t.frontiera = [0]; //frontiera, inizialmente contiene lo stato attuale, cioè il nodo 0 in A* corrisponde a "open"
t.pesi = [0]; //peso d'entrata di ogni nodo, corrisponde al numero di mosse necessarie per raggiungerlo dalla radice
t.padre = ["null"]; //ovviamente nessun padre, questo nodo è la radice
t.mossa = ["null"]; //e nessuna mossa è stata fatta per trovarci in questo stato
t.stati = [t.stato]; //vettore con tutte le configurazioni degli stati visitati
eu = f(0); //euristica dello stato iniziale
t.euristica = [eu]; //la inserisco nel vettore delle euristiche
attuale = 0; //il nodo che devo visitare
t.ultimo = 0; //l'ultimo nodo inserito nell'albero
echo "Pianificazione delle azioni da eseguire...";
prog = 0;
while (temp != t.goal) { //O(n^2)
	prog = prog + 1;
	echo "	Passo " + prog + ":";
	t.frontiera = togli(t.frontiera, attuale); //tolgo dalla frontiera il nodo che visito
	if (t.mossa[attuale] != t.colori[1]) { //evito di eseguire 2 volte di seguito la stessa mossa: mi riporterebbe un passo indietro
		t.ultimo = t.ultimo + 1; //primo valore disponibile per un nodo da inserire
		t.padre = t.padre + attuale; //imposto il nodo corrente come padre di quello appena aggiunto
		t.mossa = t.mossa + t.colori[1]; //ho raggiunto questo stato spostando la palla blu
		papi = t.stati[t.padre[t.ultimo]];
		st = scambia(papi, t.colori[1]);
		t.stati = t.stati + st;
    	esiste = stessopadre(t.ultimo);
		if (esiste) { //se trovo un padre uguale
			t.padre = togliultimo(t.padre); //dal vettore dei padre
			t.mossa = togliultimo(t.mossa);   //e da quello delle mosse
			t.stati = togliultimo(t.stati);
			t.ultimo = t.ultimo - 1; //tolgo questo nodo
		} else { //se non c'è un padre uguale
			t.frontiera = t.frontiera + t.ultimo; //inserisco in frontiera il nuovo nodo
			peso = g(t.ultimo); //calcolo il peso di questo nodo
			t.pesi = t.pesi + peso; //lo aggiungo nel vettore dei pesi
			eu = f(t.ultimo); //calcolo l'euristica di questo nuovo nodo
			t.euristica = t.euristica + eu //la inserisco nel vettore delle euristiche
			}
	};
	if (t.mossa[attuale] != t.colori[2]) { //idem per la mossa rosso
		t.ultimo = t.ultimo + 1;
		t.padre = t.padre + attuale;
		t.mossa = t.mossa + t.colori[2];
		papi = t.stati[t.padre[t.ultimo]];
		st = scambia(papi, t.colori[2]);
		t.stati = t.stati + st;
		esiste = stessopadre(t.ultimo);
		if (esiste) {
			t.padre = togliultimo(t.padre);
			t.mossa = togliultimo(t.mossa);
			t.stati = togliultimo(t.stati);
			t.ultimo = t.ultimo - 1;
		} else {
			t.frontiera = t.frontiera + t.ultimo;
			peso = g(t.ultimo);
			t.pesi = t.pesi + peso;
			eu = f(t.ultimo);
			t.euristica = t.euristica + eu
			}
	};
	if (t.mossa[attuale] != t.colori[3]) { //idem per la mossa altro
		t.ultimo = t.ultimo + 1;
		t.padre = t.padre + attuale;
		t.mossa = t.mossa + t.colori[3];
		papi = t.stati[t.padre[t.ultimo]];
		st = scambia(papi, t.colori[3]);
		t.stati = t.stati + st;
		esiste = stessopadre(t.ultimo);
		if (esiste) {
			t.padre = togliultimo(t.padre);
			t.mossa = togliultimo(t.mossa);
			t.stati = togliultimo(t.stati);
			t.ultimo = t.ultimo - 1;
		} else { //se non c'è un padre uguale
			t.frontiera = t.frontiera + t.ultimo;
			peso = g(t.ultimo);
			t.pesi = t.pesi + peso;
			eu = f(t.ultimo);
			t.euristica = t.euristica + eu
		}
	};
	attuale = eurmin(t.frontiera); //il nuovo nodo è quello con euristica migliore
	temp = t.stati[attuale]; //simulo lo stato attuale per poter successivamente calcolare le euristiche

	echo "	Miglior stato trovato: [" + temp[0] + " ; " + temp[1] + " ; " + temp[2] + " ; " + temp[3] + "]";
};
echo "Pianificazione eseguita.";
echo "Stato da raggiugere: [" + temp[0] + " ; " + temp[1] + " ; " + temp[2] + " ; " + temp[3] + "]"; //ho calcolato giusto??

//ESEGUO IL PIANO
dafare = elencomosse(attuale); //mosse da eseguire per passare dallo stato attuale a quello obiettivo
str = "Mosse da eseguire: [";
i = conta(dafare);
if (i > 0) {
	k = 1;
	foreach n in dafare {
		if (k == i) {
			str = str + n
		} else {
			str = str + n + " ; "
		};
		k = k + 1;
	}
} else {
	str = str + "nessuna"
};
echo str + "]";
foreach n in dafare { //eseguo le mosse nell'ordine
	azzera();
	sposta(n)
};
echo "Stato finale: [" + t.stato[0] + " ; " + t.stato[1] + " ; " + t.stato[2] + " ; " + t.stato[3] + "]"; //sono arrivato nello stato giusto??