#include "sim_execution.h"

rgbhue pix;  // color to be searched
VWHandle vwhandle;
PSDHandle psdF, psdL, psdR, psdB;
int found; // Checks to see if the ball was seen. If it is, a bonus is given to fitness

int main(int argc, char *argv[])
{
	/*** declaration stuff for the lisp_parser main ***/
	Tree t;
	double init_dist, end_dist, fitness, timed_out = false;
	FILE *fitness_file;

	found = false;
    	pix.red=NO_HUE; /* set color to be searched */
        pix.blue=NO_HUE;
        pix.green=NO_HUE;
        pix.hue = RED_HUE;

	if(initVW(&vwhandle) == false)
	{
		LCDPrintf("error in initVW()... \n");
		return false;
	}
	camInit();
	initPSD(&psdR, &psdL, &psdR, &psdB);

	LCDPrintf("Main Program\n");

	t = CreateTree("runme.lsp");

	init_dist = distance();
	if (compute(t->root) == TIMED_OUT) { printf("Program timed out\n"); timed_out = true; }
	end_dist = distance();

	/* Fitness is calculated as the difference between the starting distance and the ending distance.
	Hence, the higher the value, the better */
	fitness = init_dist - end_dist;
	// The fitness function is just initial distance - ending distance. Keeping it simple seems better! 
	fitness_file = fopen ("fitness.txt", "w");
	if (fitness_file == NULL) { printf("Error opening fitness file!!\n"); exit(1); }
	fprintf(fitness_file,"%f\n",fitness);
	fclose(fitness_file);
	fprintf(stderr,"Initial ball dist. %f, final dist. %f  FITNESS: %f\n", init_dist, end_dist, fitness);

	releaseVW(&vwhandle);
	releasePSD();
	camRelease();

	exit(0);
}