/* getTrajectoryParameters
 * -----------------------
 */
void getTrajectoryParameters(int max, vector_t* traj,
			     float* a, float* b, float* c, float* d) {
  float* data_xx;
  float* data_yy;
  int i;

  // create data_xx and data_yy
  data_xx = (float*) malloc(sizeof(float) * max);
  data_yy = (float*) malloc(sizeof(float) * max);
  for (i = 0; i < max; i++) {
    data_xx[i] = traj[i].x;
    data_yy[i] = traj[i].y;
  }

  compute_least_square(max, data_xx, data_yy, a, b, c, d);
  free(data_xx);
  free(data_yy);
}

/* getDisparity
 * ------------
 * Computes the disparity between curve1 (defined as ax^2 + bx + c)
 * and curve2 (defined by d, e, f) by computing the affine 
 * transformation (i.e., the scalex and scaley) that takes
 * curve2 to curve1 and comparing it to the identity.
 */
float getDisparity(float a, float b, float c, 
		   float d, float e, float f)
{
  float disparity;
  float ymax1, ymax2; // the height of the diver at the peak
  float ymin1, ymin2;
  float scalex, scaley;

  ymax1 = -((b * b) / (4 * a)) + c;
  ymax2 = -((e * e) / (4 * d)) + f; 
  scaley = ymax2 / ymax1;

  // pick an arbitrary x that is in the down part of the trajectory (e.g.,
  // x = -b)
  ymin1 = (b * b * (a - 1)) + c;
  ymin2 = (e * e * (d - 1)) + f;
  scalex = ymin2 / ymin1;

  scaley -= 1.0;
  scalex -= 1.0;
  disparity = ((scaley * scaley) + (scalex * scalex)); // sqr to get positive
	
  return (disparity);
}



void printTrajectory(FILE* fp, int max, vector_t* traj) {
  int i;
  fprintf(fp, "%% matlab-compatible output:\n");
  fprintf(fp, "figure(1); clf;\n");
  fprintf(fp, "traj = [ ");
  for (i = 0; i < max; i++) {
    fprintf(fp, "%3.3f, %3.3f; ...\n", traj[i].x, traj[i].y);
  }
  fprintf(fp, " ];\n");
  fprintf(fp, "plot(traj(:,1),%d-traj(:,2),'b+');\n", IMAGE_HEIGHT); // flip
}

void printCurve(FILE* fp,
		int m, vector_t* traj, float a, float b, float c, float d) {
  int i;
  float min, max;
  min = max = traj[0].x;
  for (i = 1; i < m; i++) {
    if (traj[i].x > max) {
      max = traj[i].x;
    }
    if (traj[i].x < min) {
      min = traj[i].x;
    }
  }

  fprintf(fp, "a = %f; b = %f; c = %f; d = %f;\n", a, b, c, d);
  fprintf(fp, "xx = %f:%f:%f;\n", min, (max - min) / m, max);
  fprintf(fp, "yy = a * xx.^3 + b * xx.^2 + c * xx + d;\n");
  fprintf(fp, "hold on; plot(xx,%d-yy,'r:');\n", IMAGE_HEIGHT); // flip
  fprintf(fp, "title('Trajectory'); xlabel('x'); ylabel('y');\n");
  fprintf(fp, "axis([0 %d 0 %d]);\n", IMAGE_WIDTH, IMAGE_HEIGHT);
}
