#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <sys/time.h>
#include <unistd.h>
#include <signal.h>

#define SQR(X)		(X*X)
#define MIN(X,Y) 	((X < Y)? X:Y)
#define DegToRad 0.017453292   

typedef struct {
  int x;
  int y;
  short theta;
} Point;

typedef struct {
  int x1;
  int y1;
  int x2;
  int y2;
} Segment;

typedef struct {
  int x;
  int y;
} Points;


typedef struct {
  int x;
  int  y;
  float radians;
} Robot;


Segment *wld;
int NumSeg = 0, X_1, X_2, Y_1, Y_2, width = 0, height = 0, temp;
float robo_x, robo_y, grids = 500, cells = 500;
float robo_phi, *errors, minum = 0, mean = 0, median, calc;
Point stack[100];
Points *verify;
char fdata[50], tmp[50], tmp1[50];
Robot robot;

void DepthSort(float *tmp, int val)  {
  float max, other;
  int i, j, mark;

  for(i = val-1; i > 0; i--) {
    max = tmp[0];
    mark = 0;
    for(j = 1; j <= i;  j++)
      if(tmp[j] > max)  {
        mark = j;
        max  = tmp[j];
      }
    if (mark != i) {
      other = tmp[mark];
      tmp[mark] = tmp[i];
      tmp[i] = other;
    }
  }
}

int World(char *file) {
  char ch;

  Segment tmpSeg;
  int cnt, pop;
  int pushed = 0;
  int tmpx, tmpy;
  FILE *fp;

  if (file == (char *)NULL)
    return 1;
  if ((fp = fopen(file, "r")) == NULL) {
    fprintf(stderr, "\nCannot open %s\n", file);
    return 1;
  }
  NumSeg = 0;
  if (wld != (Segment *)NULL)
    free(wld);
  while (!feof(fp)) {
    fscanf(fp, "%[^\n]", fdata);
    if (!feof(fp))
      fscanf(fp, "%c", &ch);
    if (*fdata != '\0') {
      cnt = 0;
      while (isspace(fdata[cnt]))
        cnt++;
      if (fdata[cnt] != ';') {
        if (!strncmp(fdata+cnt, "width", 5))
          sscanf(fdata+cnt, "%s %ld", tmp, &width);
        else if (!strncmp(fdata+cnt, "height", 6))
          sscanf(fdata+cnt, "%s %ld", tmp, &height);
        else if ((!strncmp(fdata + cnt, "push", 4)) || 
		(!strncmp(fdata + cnt, "change", 6))) {
          sscanf(fdata + cnt, "%s %ld %ld %hd", tmp, &stack[pushed].x, 
		&stack[pushed].y, &stack[pushed].theta);
          pushed++;
        } else if (!strncmp(fdata + cnt, "pop", 3)) {
          pushed--;
          if (pushed < 0) {
            fprintf(stderr, "Error in format: Excess Pop\n");
            return 1;
          }
        } else if (!strncmp(fdata + cnt, "position", 8)) {
          sscanf(fdata + cnt, "%s %d %d %f", tmp, &robot.x, &robot.y, 
                         &robot.radians);
          robot.radians = 180 / M_PI * robot.radians;
        } else if (isdigit(fdata[cnt]) || (fdata[cnt] == '-')) {
          if (NumSeg)
            wld = (Segment *)realloc(wld, (NumSeg + 1) * sizeof(Segment));
          else {
            NumSeg++;
            wld = (Segment *)malloc(2 * sizeof(Segment));
          }
          sscanf(fdata + cnt, "%ld %ld %ld %ld", &wld[NumSeg].x1, 
		&wld[NumSeg].y1, &wld[NumSeg].x2, &wld[NumSeg].y2);
          for(pop = pushed - 1; pop >= 0; pop--) {
            tmpx = wld[NumSeg].x1;
            tmpy = wld[NumSeg].y1;
            wld[NumSeg].x1 = (int)(stack[pop].x + tmpx * cos(DegToRad * 
		stack[pop].theta) + tmpy * sin(DegToRad * stack[pop].theta));
            wld[NumSeg].y1 = (int)(stack[pop].y + tmpy * cos(DegToRad * 
		stack[pop].theta) - tmpx * sin(DegToRad * stack[pop].theta));
            tmpx = wld[NumSeg].x2;
            tmpy = wld[NumSeg].y2;
            wld[NumSeg].x2 = (int)(stack[pop].x + tmpx*cos(DegToRad * 
		stack[pop].theta) + tmpy * sin(DegToRad * stack[pop].theta));
            wld[NumSeg].y2 = (int)(stack[pop].y + tmpy * cos(DegToRad * 
		stack[pop].theta) - tmpx * sin(DegToRad * stack[pop].theta));
          }
          if (wld[NumSeg].x2 < wld[NumSeg].x1) {
            tmpx = wld[NumSeg].x1;
            tmpy = wld[NumSeg].y1;
            wld[NumSeg].x1 = wld[NumSeg].x2;
            wld[NumSeg].y1 = wld[NumSeg].y2;
            wld[NumSeg].x2 = tmpx;
            wld[NumSeg].y2 = tmpy;
          }
          else {
            wld[NumSeg].y1 = wld[NumSeg].y1;
            wld[NumSeg].y2 = wld[NumSeg].y2;
          }
          NumSeg++;
        }
      }
    }
    *fdata = '\0';
  }
  fclose(fp);
  for(cnt=2; cnt<NumSeg; cnt++) {
    pop = 1;
    while(pop != -1)
      if (wld[pop].x1 < wld[cnt].x1)
        pop = (cnt - pop > 1)? pop + 1 : -1;
      else {
        memcpy(&tmpSeg, wld + cnt, sizeof(Segment));
        for(pushed = cnt; pushed > pop; pushed--)
          memcpy(wld + pushed, wld + pushed - 1, sizeof(Segment));
        memcpy(wld + pop, &tmpSeg, sizeof(Segment));
        pop = -1;
      }
  }
  memcpy(wld, wld + 1, sizeof(Segment));
  for(cnt = 0; cnt < NumSeg; cnt++)
    fprintf(stderr,"POS(%d) %ld %ld - %ld %ld\n", cnt, wld[cnt].x1, 
	wld[cnt].y1,wld[cnt].x2, wld[cnt].y2);

  return 0;
}

int main(int argc, char **argv)  {
  int i = 0, j, k, count = 0, sim = 1, ref = 0;
  int max_val, max_num;
  Segment control, test;
  char *strings, ch[10];

  strings = (char *)calloc(80, sizeof(char));

  argv++;
  if(argc >= 2)
    if(strncmp("REAL", *argv, 4) == 0)  {
      sim = 0;
      argv++;
      argc--;
    } else if (strncmp("SIM", *argv, 3) == 0) {
      argv++;
      argc--;
    }

  /* Storing the Control Map */
  if(argc >= 2)  {
    i = World(*argv);
    argv++;
  } else {
    printf("\nInput the original map name :  ");
    scanf("%s",strings);
    i = World(strings);
  }
  while(i != 0)  {
    printf("\nMap unsuccessfully loaded.  Input map again :  ");
    scanf("%s",strings);
    i = World(strings);
  }

    verify = (Points *)calloc(2 * NumSeg, sizeof(Points));

    /* Store Critical Points from Control Map */
    for(i = 0; i < NumSeg; i++)  {
      verify[count].x = wld[i].x1;
      verify[count].y = wld[i].y1;
      k = 1;
      for(j = 0; j < count; j++)
        if((verify[count].x == verify[j].x) && (verify[count].y == verify[j].y))
 	  k = 0;
      count += k;
      verify[count].x = wld[i].x2;
      verify[count].y = wld[i].y2;
      k = 1;
      for(j=0; j<count; j++)
        if((verify[count].x == verify[j].x) && (verify[count].y == verify[j].y))
          k = 0;
      count += k;
    }

  for(i=0;i<count;i++)
    printf("Point %d  %d\n",verify[i].x, verify[i].y);

  /* Eliminate any corners - for simulator */
  if (sim) {
    printf("\n\n Map has %d critical points\n", count);
    do {
      printf("\nIgnore any critical points? :");
      scanf("%d", &i);
      if (i != 0)  {
        --count;
        for(j = i - 1; j < count; j++) {
          verify[j].x = verify[j + 1].x; 
          verify[j].y = verify[j + 1].y; 
        }
        for(i=0;i<count;i++)
          printf("Point %d  %d\n",verify[i].x, verify[i].y);
        printf("\n\n Map has %d critical points\n", count);
      }
    } while (i != 0);

  /* Obtain the reference line for the control map */
/*  } else {
    do {
      printf("\n select a reference line : ");
      scanf("%d", &ref);
    } while(ref >= NumSeg); 
    control.x1 = wld[ref].x1;
    control.x2 = wld[ref].x2;
    control.y1 = wld[ref].y1;
    control.y2 = wld[ref].y2;
*/
  }

  /* Store Generated Map */
  if(argc >= 3) 
    i = World(*argv);
  else {    printf("\nInput the generated map name :  ");
    scanf("%s",strings);
    i = World(strings);
  }

  while(i != 0)  {
    printf("\nMap unsuccessfully loaded.  Input map again :  ");
    scanf("%s",strings);
    i = World(strings);
  }

  /* Obtain values to define the robot position */
  if (sim) {
    printf("\nInput the robot coordinates\n");
    printf("X : ");
    scanf("%f", &robo_x);
    printf("Y : ");
    scanf("%f", &robo_y);
    printf("PHI : ");
    scanf("%f", &robo_phi);

    printf("\n\nGrid specifications : ");
    scanf("%f", &grids);
    printf("Cell width : ");
    scanf("%f", &cells);

    /* Translate coordinates to match previous reference system */
    for(i = 0; i < NumSeg; i++)  {
      temp      = cos(robo_phi) * (wld[i].x1 - (grids / 2 * cells) - 100) -
		  sin(robo_phi) * (wld[i].y1 - (grids / 2 * cells) - 100) + 
		  robo_x;
      wld[i].y1 = cos(robo_phi) * (wld[i].y1 - (grids / 2 * cells) - 100) + 
		  sin(robo_phi) * (wld[i].x1 - (grids / 2 * cells) - 100) + 
		  robo_y;
      wld[i].x1 = temp;
      temp      = cos(robo_phi) * (wld[i].x2 - (grids / 2 * cells) - 100) -
	 	  sin(robo_phi) * (wld[i].y2 - (grids / 2 * cells) - 100) + 
		  robo_x;
      wld[i].y2 = cos(robo_phi) * (wld[i].y2 - (grids / 2 * cells) - 100) + 
		  sin(robo_phi) * (wld[i].x2 - (grids / 2 * cells) - 100) + 
  	 	  robo_y;
      wld[i].x2 = temp;

      printf("%d %d <> %d %d\n", wld[i].x1, wld[i].y1, wld[i].x2, wld[i].y2);
    } 

  /* Referencing for real environment */
  } else {
    printf("\n\nInitial Point [ulrd] : ");
    scanf("%s", ch);
    switch (ch[0])  {
      case 'l':
        max_val = 100000;
	for(i = 0; i < NumSeg; i++) {
	  if(wld[i].x1 < max_val) {
	    max_val = wld[i].x1;
	    test.x1 = wld[i].x1;
	    test.y1 = wld[i].y1;
	  }
	  if(wld[i].x2 < max_val) {
	    max_val = wld[i].x2;
	    test.x1 = wld[i].x2;
	    test.y1 = wld[i].y2;
	  }
        }
	break;
      case 'r':
        max_val = 0;
	for(i = 0; i < NumSeg; i++) {
	  if(wld[i].x1 > max_val) {
	    max_val = wld[i].x1;
	    test.x1 = wld[i].x1;
	    test.y1 = wld[i].y1;
	  }
	  if(wld[i].x2 > max_val) {
	    max_val = wld[i].x2;
	    test.x1 = wld[i].x2;
	    test.y1 = wld[i].y2;
	  }
        }
     	break;
      case 'd':
        max_val = 100000;
	for(i = 0; i < NumSeg; i++) {
	  if(wld[i].y1 < max_val) {
	    max_val = wld[i].y1;
	    test.x1 = wld[i].x1;
	    test.y1 = wld[i].y1;
	  }
	  if(wld[i].y2 < max_val) {
	    max_val = wld[i].y2;
	    test.x1 = wld[i].x2;
	    test.y1 = wld[i].y2;
	  }
        }
	break;
      case 'u':
      default:
        max_val = 0;
	for(i = 0; i < NumSeg; i++) {
	  if(wld[i].y1 > max_val) {
	    max_val = wld[i].y1;
	    test.x1 = wld[i].x1;
	    test.y1 = wld[i].y1;
	  }
	  if(wld[i].y2 > max_val) {
	    max_val = wld[i].y2;
	    test.x1 = wld[i].x2;
	    test.y1 = wld[i].y2;
	  }
        }
	break;
      }

    printf("\n\nFinal Point [ulrd] : ");
    scanf("%s", ch);
    switch (ch[0])  {
      case 'l':
        max_val = 100000;
	for(i = 0; i < NumSeg; i++) {
	  if(wld[i].x1 < max_val) {
	    max_val = wld[i].x1;
	    test.x2 = wld[i].x1;
	    test.y2 = wld[i].y1;
	  }
	  if(wld[i].x2 < max_val) {
	    max_val = wld[i].x2;
	    test.x2 = wld[i].x2;
	    test.y2 = wld[i].y2;
	  }
        }
	break;
      case 'r':
        max_val = 0;
	for(i = 0; i < NumSeg; i++) {
	  if(wld[i].x1 > max_val) {
	    max_val = wld[i].x1;
	    test.x2 = wld[i].x1;
	    test.y2 = wld[i].y1;
	  }
	  if(wld[i].x2 > max_val) {
	    max_val = wld[i].x2;
	    test.x2 = wld[i].x2;
	    test.y2 = wld[i].y2;
	  }
        }
     	break;
      case 'd':
        max_val = 100000;
	for(i = 0; i < NumSeg; i++) {
	  if(wld[i].y1 < max_val) {
	    max_val = wld[i].y1;
	    test.x2 = wld[i].x1;
	    test.y2 = wld[i].y1;
	  }
	  if(wld[i].y2 < max_val) {
	    max_val = wld[i].y2;
	    test.x2 = wld[i].x2;
	    test.y2 = wld[i].y2;
	  }
        }
	break;
      case 'u':
      default:
        max_val = 0;
	for(i = 0; i < NumSeg; i++) {
	  if(wld[i].y1 > max_val) {
	    max_val = wld[i].y1;
	    test.x2 = wld[i].x1;
	    test.y2 = wld[i].y1;
	  }
	  if(wld[i].y2 > max_val) {
	    max_val = wld[i].y2;
	    test.x2 = wld[i].x2;
	    test.y2 = wld[i].y2;
	  }
        }
	break;
    }
    control.x2 = test.x2 - test.x1;
    control.y2 = test.y2 - test.y1;
    control.x1 = 0;
    control.y1 = 0;
    if(control.x2 == 0)
      if(control.y2 >= 0)
        robo_phi = M_PI / 2;
      else 
        robo_phi = -M_PI / 2;
    else {
      robo_phi = atan((float)control.y2/(float)abs(control.x2));
      if(control.x2 < 0)
        robo_phi = M_PI - robo_phi;
    }
    robo_phi = -robo_phi;
    max_val = 0;
    max_num = 0;
    for(i = 0; i < NumSeg; i++)  {
      temp	= cos(robo_phi) * (wld[i].x1 - test.x1) -
		  sin(robo_phi) * (wld[i].y1 - test.y1);
      wld[i].y1 = cos(robo_phi) * (wld[i].y1 - test.y1) +
                  sin(robo_phi) * (wld[i].x1 - test.x1);
      wld[i].x1 = temp;
      if(temp < max_val)
        max_val = temp;
      temp      = cos(robo_phi) * (wld[i].x2 - test.x1) -
                  sin(robo_phi) * (wld[i].y2 - test.y1);
      wld[i].y2 = cos(robo_phi) * (wld[i].y2 - test.y1) +
                  sin(robo_phi) * (wld[i].x2 - test.x1);
      wld[i].x2 = temp;
      if(temp < max_val)
        max_val = temp;
    }
    for(i=0;i<NumSeg;i++) {
      wld[i].x1 -= max_val;
      wld[i].x2 -= max_val;
    }
  }
  
  errors = (float *)calloc(count, sizeof(float));
  for(i = 0; i < count; i++)  {
    minum = cells * grids;
    for(j = 0; j < NumSeg; j++) {
      calc = sqrt((wld[j].x1 - verify[i].x) * (wld[j].x1 - verify[i].x) + 
		(wld[j].y1 - verify[i].y) * (wld[j].y1 - verify[i].y));
      if(calc < minum)
        minum = calc;
      calc = sqrt((wld[j].x2 - verify[i].x) * (wld[j].x2 - verify[i].x) + 
		(wld[j].y2 - verify[i].y) * (wld[j].y2 - verify[i].y));
      if(calc < minum)
        minum = calc;
    }
    errors[i] = minum;
    mean += minum;
  }
  mean /= (float)count;
  printf("\n\n\n");
 
  printf("The mean error is %f pixels/corner\n",mean); 
  DepthSort(&errors[0], count);
  printf("\n\t%f median\n",errors[count/2]);

  return 0;
}