#include "sim_execution.h"
#include <math.h>
#include <stdio.h>
#define SQUARE(a)	((a)*(a))
#define MIN(a,b)	(a<b?a:b)
#define MAX(a,b)	(a>b?a:b)
#define MAX_SIMSTEPS 200

static int timer = 0;
// Need to use a sim_timer since it is possible a move function would not be called, and hence it will never timeout.
static int sim_timer = 0;

extern VWHandle vwhandle;
extern PSDHandle psdF, psdB, psdL, psdR;
extern rgbhue pix;  // color to be searched
extern char lname[16][20];
extern int found;

int Convert2Sym (char *symbol_string) {
	int i;
	for (i=0;i<16;i++) {
		if (!strcmp(symbol_string, lname[i])) return i;
	}
	fprintf(stderr,"ERROR: Undefined symbol! >%s<\n",symbol_string);
	exit(1);
}

int isletter(char c)
// return true is c is A..Z or a..z
{ c = toupper(c);
  return ('A' < c && c < 'Z');
}

Tree CreateTree(char *filename)
// Read complete program from LISP file
// and return as Tree struture
{	Tree t;
	FILE *f;
	char c='0';
	char symbol_string[20];
	int symbol;

	t = initTree();
	f = fopen (filename, "r");
	if (f == NULL) fprintf(stderr,"Error opening file\n");

	while (c != EOF)
	{ c = fgetc(f);
	  while (!isletter(c))
	  { c = fgetc(f); //skip all non-letters
	    if (c == EOF) { if (DEBUG) printf("===EOF===\n"); break;}
	  }
	  ungetc(c,f);
	  fscanf(f, "%s", symbol_string); // read next symbol string
	  if (DEBUG) printf("read symbol from file: %s\n", symbol_string);
	  symbol = Convert2Sym(symbol_string);
	  add2Tree (t, symbol);
	}
	fclose(f);
	return t;
}

int RGB2Hue (BYTE r, BYTE g, BYTE b) {
	int hue, delta, max, min;

	max = MAX(r, MAX(g,b));
	min = MIN(r, MIN(g,b));
	delta = max-min;
	hue = 0;

	if (2*delta <= max) hue = NO_HUE;
        else {
		if (r==max) hue = 42 + 42*(g-b)/delta;
		else if (g==max) hue = 126 + 42*(b-r)/delta;
		else if (b==max) hue = 210 + 42*(r-g)/delta;
	}
	return (BYTE) hue;
}

void ColSearch (colimage img, int obj_hue, int thres, int *pos, int *val)
{ int x, y, count, h, distance;
	*pos = -1; *val = 0;
	for (x=0;x<imagecolumns;x++) {
		count = 0;
		for (y=0;y<imagerows;y++) {
			h = RGB2Hue(img[y][x][0], img[y][x][1], img[y][x][2]);

			if (h != NO_HUE) {
				distance = abs ((int)h-obj_hue);
				if (distance > 126) distance = 253 - distance;
				if (distance < thres) count++;
			}
		}
	}
}

int compute(Node n)
// runs a LISP program recursively for max. number of steps
// returns: data value of symbol, -1 for function
#define epsilon 0.2  // ball distance at which SUCCESS is signalled
{ int ret, return_val1, return_val2;
  lastsizes libz;
  colimage img;
  int val, pos;
  double bd;

  sim_timer++;
  if (timer > SIM_TIMEOUT) return TIMED_OUT;
  if (sim_timer > MAX_SIMSTEPS) return TIMED_OUT;
  if (VWStalled(vwhandle)) return TIMED_OUT;

  // stop simulation if ball has been reached
  bd = distance();
  if (bd<epsilon) 
  { fprintf(stderr, "Success, ball distance = %f\n", bd);
    return TIMED_OUT;
  }

	if (DEBUG) printf("Timer is at %d\n",timer);

	initSizelib(&libz);
	CAMGetColFrame (&img, 0);
	LCDPutColorGraphic(&img);
	ColSearch2 (img, RED_HUE, 10, &pos, &val);
	if (pos>0) pos = 0;
	LCDSetPos(1,0);
	LCDPrintf("p%2d v%2d\n", pos, val);

	if (val>0) found = true;

	ret = -1; /* no return value */

	if (DEBUG) printf("Symbol: %s (%d)\n", lname[n->symbol], n->symbol);
	switch(n->symbol)
	{
		case PROGN2:
		  if (DEBUG) printf("PROGN2 ................\n");
		  compute(n->children[0]);
		  compute(n->children[1]);
		  break;

		case IF_LESS:
		  if (DEBUG) printf("IF +++++++++++++\n");
		  return_val1 = compute(n->children[0]);
		  return_val2 = compute(n->children[1]);
		  if (return_val1 == TIMED_OUT || return_val2 == TIMED_OUT) return TIMED_OUT;
		  if (DEBUG) printf("val1 %d < val2 %d\n",return_val1, return_val2);
		  if (return_val1 < return_val2) compute(n->children[2]);
		    else                          compute(n->children[3]);
		  break;

		case WHILE_LESS:
		  do {
		    if (DEBUG) printf("WHILE ************\n");
		    return_val1 = compute(n->children[0]);
		    return_val2 = compute(n->children[1]);
		    if (return_val1 == TIMED_OUT || return_val2 == TIMED_OUT) return TIMED_OUT;
		    if (DEBUG) printf("val1 %d < val2 %d\n",return_val1, return_val2);
		    if (return_val1 < return_val2) compute(n->children[2]); /* execute WHILE loop body */
		  } while (return_val1 < return_val2);
		  if (DEBUG) printf("******** END WHILE\n");
		  break;

		case turnleft: timer++; turn_left(&vwhandle);
		  break;
		case turnright: timer++; turn_right(&vwhandle);
		  break;
		case moveforward: timer++; move_forward(&vwhandle);
		  break;
		case movebackward: timer++; move_backward(&vwhandle);
		  break;

		case psdfront:
			ret = PSDGet(psdF);
			if (DEBUG) printf("execute: psdfront %d\n", ret);
			break;
//		case psdback:
//			ret = PSDGet(psdB);
//			if (DEBUG) printf("execute: psdback %d\n", ret);
//			break;
		case psdleft:
			ret = PSDGet(psdL);
			if (DEBUG) printf("execute: psdleft %d\n", ret);
			break;
		case psdright:
			ret = PSDGet(psdR);
			if (DEBUG) printf("execute: psdright %d\n", ret);
			break;

		case objsize:
			ColSearch2 (img, RED_HUE, 10, &pos, &ret);
			if (ret<0) ret = 0;
			if (DEBUG) printf("ColSearch pos %d SIZE %d\n", pos, ret);
			break;
		case objpos:
			ColSearch2 (img, RED_HUE, 10, &ret, &val);
			if (ret<0) ret = 0;
			if (DEBUG) printf("ColSearch POS %d size %d\n", ret, val);
			break;

		case low:  ret = LOW;
			break;
		case high: ret = HIGH;
			break;
		case msd:  ret = MINIMUM_SAFE_DIS;
			  break;

		default: fprintf(stderr, "ERROR in execute, symbol %d\n", n->symbol);
                 if (n->symbol < 20)  fprintf(stderr,"     %s\n", lname[n->symbol]);
			 exit(1);
	}

	if(DEBUG) printf("RET: %d\n", ret);
	if(DEBUG) printf("Distance from the ball = %f\n",distance());
	return ret;
}



double distance(void)
/* Calculates the distance the eyebot is from the ball */
{
	double robx, roby, ballx, bally;

	SIMGetActualPos (&robx, &roby);
	SIMGetActualBallPos (0, &ballx, &bally);
	if (DEBUG) printf("Robot position is %f %f\n",robx, roby);
	if (DEBUG) printf("Ball position is %f %f\n",ballx, bally);
	return sqrt (SQUARE(robx-ballx) + SQUARE(roby-bally));
}