#include <stdio.h>
#include <stdlib.h>
#include <ctype.h>
#include <math.h>

typedef struct meandev_ {
  int num; /* Number of parsed items */
  double dist; /* Mean distance */
  double ang; /* Mean angle */
  double dstdev;
  double astdev;
  double ldstdev; /* Low dist standard deviation (below mean) */
  double hdstdev; /* High (>= mean) */
  double lastdev;
  double hastdev;
} meandev;

const double codist[] = {3000.0, 2400.0, 1800.0, 1200.0, 600.0};
const double cedist[] = {1666.7, 1333.3, 1000.0, 666.7, 333.3};
const int iscorner[] = {1, 1, 0, 0, 1, 1};


void getquad(double x1, double y1, double x2, double y2, double x3, double y3,
	     double quad[3]) {
  /* f(x) = quad[0]x^2 + quad[1]x + quad[2] */
  double a,b,c;
  if (!quad) return;
  a = (y3 - y2)/((x3-x2)*(x3-x1)) - (y1-y2)/((x1-x2)*(x3-x1));
/*   b = (y3 - y2 + a*(x2*x2-x3*x3))/(x2-x3); */
/*   c = y3-a*x3*x3 - b*x3; */
/*   printf("%fx^2 + %fx + %f\n",a,b,c); */
  b = (y1 - y2 + a*(x2*x2-x1*x1))/(x1-x2);
/*   c = y2-a*x2*x2 - b*x2; */
/*   printf("or %fx + %f\n",b,c); */
  c = y1-a*x1*x1 - b*x1;
/*   printf("or %f\n",c); */
  quad[0] = a;
  quad[1] = b;
  quad[2] = c;
}

void getline(const double *x, const double *y, int num, double line[2]) {
  /* y = line[0]*x + line[1] */
  double d_1 = 0.0;
  
  double xs = 0.0, ys = 0.0, x2s = 0.0, xys = 0.0;

  int i;

  if (!line) return;

  for (i = 0; i < num; i++) xs += x[i];
  for (i = 0; i < num; i++) ys += y[i];
  for (i = 0; i < num; i++) x2s += x[i]*x[i];
  for (i = 0; i < num; i++) xys += x[i]*y[i];
  
  d_1 = 1.0 / (num*x2s-(xs*xs));
  line[0] = d_1*(num*xys - xs*ys);
  line[1] = d_1*(x2s*ys - xs*xys);
}

double geterror(double *x, double *y, int num, const double line[2]) {
  int i;
  double theta = 0;
  for (i = 0; i < num; i++) {
    double tmp = line[0]*x[i] + line[1] - y[i];
    theta += tmp*tmp;
  }
  return theta;
}

void getmeandev(FILE *file, int seq, meandev *mymd) {
  char buf[1024];
  char *startptr;
  char *endptr;
  double meanconf = 0.0, meandist = 0.0, meanang = 0.0;
  double lmeandist = 0.0, lmeanang = 0.0;
  double hmeandist = 0.0, hmeanang = 0.0;
  double devdist = 0.0, devang = 0.0;
  double ldevdist = 0.0, hdevdist = 0.0, ldevang = 0.0, hdevang = 0.0;
  double curconf, tmp, tmp2;
  int numitems = 0, numsmall = 0, numbig = 0;
  int lineno = 0;

  if (!mymd) return;
  printf("Processing sequence #%d (marker %d, distance %.1f cm)\n",seq,
	 seq/5, (iscorner[seq/5] ? codist[seq%5]/10.0 : cedist[seq%5]/10.0));
  fseek(file, 0, SEEK_SET);
  /* First pass: Average acceptable confidence */
  while (fgets(buf, sizeof(buf), file)) {
    lineno++;
    if (buf[0] != 'S' || buf[1] != ((seq/10)%10+'0') || 
	buf[2] != (seq%10+'0') || buf[3] != ' ') continue;
    numitems++;
    meanconf += strtod(buf+4, &endptr);
    if (endptr == buf+4) {
      printf("Parse error 1 phase 1 line %d\n",lineno); exit(1);
    }
    startptr = endptr + 3;
    meandist += strtod(startptr, &endptr);
    if (endptr == startptr) {
      printf("Parse error 2 phase 1 line %d\n",lineno); exit(1);
    }
    startptr = endptr + 3;
    meanang += strtod(startptr, &endptr);
    if (endptr == startptr) {
      printf("Parse error 3 phase 1 line %d\n",lineno); exit(1);
    }        
  }
  if (numitems == 0) {printf("Zero items\n"); exit(0);}
  meanconf /= ((double)numitems);
  meandist /= ((double)numitems);
  meanang /= ((double)numitems);
  printf("%d items; average confidence: %f distance: %f angle: %f\n",
	 numitems, meanconf, meandist, meanang);
  if (numitems == 1) {printf("Only one item\n"); exit(0);}

  fseek(file, 0, SEEK_SET);
  /* Second pass: Average low and high confidences */
  while (fgets(buf, sizeof(buf), file)) {
    lineno++;
    if (buf[0] != 'S' || buf[1] != ((seq/10)%10+'0') || 
	buf[2] != (seq%10+'0') || buf[3] != ' ') continue;
    tmp = strtod(buf+4, &endptr);
    if (endptr == buf+4) {
      printf("Parse error 1 phase 2 line %d\n",lineno); exit(1);
    }
    startptr = endptr + 3;
    tmp2 = strtod(startptr, &endptr);
    if (endptr == startptr) {
      printf("Parse error 2 phase 2 line %d\n",lineno); exit(1);
    }
    if (tmp < meanconf) {
      lmeandist += tmp2;
      numsmall++;
    } else {
      hmeandist += tmp2;
      numbig++;
    }
    startptr = endptr + 3;
    tmp2 = strtod(startptr, &endptr);
    if (endptr == startptr) {
      printf("Parse error 3 phase 2 line %d\n",lineno); exit(1);
    }
    if (tmp < meanconf) lmeanang += tmp2;
    else hmeanang += tmp2;
  }
  if (numsmall == 0 || numbig == 0) {printf("Not differentiated\n"); exit(1);}
  lmeandist /= ((double)numsmall); lmeanang /= ((double)numsmall);
  hmeandist /= ((double)numbig); hmeanang /= ((double)numbig);
  printf("Average low conf distance %f, angle %f\n"
	 "average high conf distance %f, angle %f\n", lmeandist, lmeanang,
	 hmeandist, hmeanang);

  fseek(file, 0, SEEK_SET);
  /* Third pass: Overall, upper, lower stdevs */
  lineno = 0;
  while (fgets(buf, sizeof(buf), file)) {
    lineno++;
    if (buf[0] != 'S' || buf[1] != ((seq/10)%10+'0') || 
	buf[2] != (seq%10+'0') || buf[3] != ' ') continue;
    curconf = strtod(buf+4, &endptr);
    if (endptr == buf+4) {
      printf("Parse error 1 phase 3 line %d\n",lineno); exit(1);
    }
    startptr = endptr + 3;
    tmp = strtod(startptr, &endptr);
    if (endptr == startptr) {
      printf("Parse error 2 phase 3 line %d\n",lineno); exit(1);
    }
    devdist += pow(tmp - meandist, 2);
    if (curconf < meanconf) ldevdist += pow(tmp - lmeandist, 2);
    else hdevdist += pow(tmp - hmeandist, 2);
    startptr = endptr + 3;
    tmp = strtod(startptr, &endptr);
    if (endptr == startptr) {
      printf("Parse error 3 phase 3 line %d\n",lineno); exit(1);
    }
    devang += pow(tmp - meanang, 2);
    if (curconf < meanconf) ldevang += pow(tmp - lmeanang, 2);
    else hdevang += pow(tmp - hmeanang, 2);
  }
  if (numsmall < 2) {ldevdist = 0.0; ldevang = 0.0;}
  else {
    ldevdist = sqrt(ldevdist/((double)(numsmall-1)));
    ldevang = sqrt(ldevang/((double)(numsmall-1)));
  }
  if (numbig < 2) {hdevdist = 0.0; hdevang = 0.0;}
  else {
    hdevdist = sqrt(hdevdist/((double)(numbig-1)));
    hdevang = sqrt(hdevang/((double)(numbig-1)));
  }
  devdist = sqrt(devdist/((double)(numitems-1)));
  devang = sqrt(devang/((double)(numitems-1)));

  mymd->num = numitems;
  mymd->dist = meandist;
  mymd->ang = meanang;
  mymd->dstdev = devdist;
  mymd->astdev = devang;
  mymd->ldstdev = ldevdist;
  mymd->hdstdev = hdevdist;
  mymd->lastdev = ldevang;
  mymd->hastdev = hdevang;
  printf("Standard deviations: distance %f, angle %f\n"
	 "small: distance %f, angle %f\n"
	 "big: distance %f, angle %f\n",devdist, devang,
	 ldevdist, ldevang, hdevdist, hdevang);
}

int main(int argc, char *argv[]) {
  FILE *file1, *file2;
  meandev mymd[30];
  double line[12][2];
  double x[5], y[5];
  int i,j;

  if (argc != 3) {
    printf("Usage: %s <input log file> <output fn header file>\n",
	   argv[0]);
    exit(1);
  }
  if ((file1 = fopen(argv[1], "r")) == NULL) {
    printf("Can't open input file %s\n",argv[1]);
    exit(1);
  }
  if ((file2 = fopen(argv[2], "w")) == NULL) {
    printf("Can't open output file %s\n",argv[1]);
    exit(1);
  }

  /* Gather data */
  for (i = 0; i < 30; i++) {
    getmeandev(file1, i, mymd+i);
  }
  fclose(file1);

  /* Generate distance scale linear regression */
  for (i = 0; i < 6; i++) {
    for (j = 0; j < 5; j++) {
      x[j] = mymd[i*5+j].dist; 
      if (iscorner[i]) y[j] = codist[j]/x[j]; 
      else y[j] = cedist[j]/x[j];
    }
    getline(x, y, 5, line[i]);
    printf("Error for scale regression %d: %f\n", i, 
	   geterror(x, y, 5, line[i]));
  }

  /* Generate standard deviation linear regression */
  /* Based on uncorrected distance! */
  for (i = 0; i < 6; i++) {
    for (j = 0; j < 5; j++) {
      x[j] = mymd[i*5+j].dist; 
      y[j] = mymd[i*5+j].dstdev;
    }
    getline(x, y, 5, line[i+6]);
    printf("Error for stdev regression %d: %f\n", i, 
	   geterror(x, y, 5, line[i+6]));
  }

  /* Output lines to file */
  fputs("#ifndef __DISTCAL_H__\n#define __DISTCAL_H__\n"
	"/* Auto generated calibration header */\n\n", file2);
  for (i = 0; i < 4; i++) {
    fputs("const double ", file2);
    fputs((i < 2 ? "scaleDist" : "stdevDist"), file2);
    putc('A'+i%2, file2);
    fputs("[6] {\n", file2);
    for (j = 0; j < 6; j++) {
      fprintf(file2, "  %E",line[j+(i<2 ? 0 : 6)][i%2]);
      if (j != 5) fputs(",\n", file2);
      else fputs("\n};\n\n", file2);
    }
  }
  fputs("\nconst double minstdevDist 1.0;\n\n#endif\n", file2);
  fclose(file2);
  return 0;
}
  /* Generate scale quadratics and output to file */
  /*  
  for (i = 0; i < 6; i++) {
    if (iscorner[i]) getquad(codist[0], codist[0]/mymd[i*3].dist, 
			     codist[1], codist[1]/mymd[i*3+1].dist,
			     codist[2], codist[2]/mymd[i*3+2].dist, quad[i]);
    else getquad(cedist[0], cedist[0]/mymd[i*3].dist, 
		 cedist[1], cedist[1]/mymd[i*3+1].dist,
		 cedist[2], cedist[2]/mymd[i*3+2].dist, quad[i]);
  }
  for (i = 0; i < 3; i++) {
    fputs("const double scaleDist", file2);
    putc('A'+i, file2);
    fputs(" {\n", file2);
    for (j = 0; j < 6; j++) {
      fprintf(file2, "  %E",quad[j][i]);
      if (j != 5) fputs(",\n", file2);
      else fputs("\n};\n\n", file2);
    }
  }
  */