/**
 *  find_light.c
 *
 *  Light calibration, with known pencil height and different pencil images.
 *
 *
 *  by Naveed Hasan <naveed@cs.columbia.edu>
 *     Paul Blaer <psb15@columbia.edu>
 *     Corey Tripp <crt17@columbia.edu>
 *     Lenny Volchok <lv71@columbia.edu>
 *
 *     Team Eclipse
 *
 *
 *  parts by Paul Bourke <pbourke@swin.edu.au> [LineLineIntersect()]
 *           Reg Willson <rgwillson@mmm.com> [readParams() & structs]
 */


#include <stdio.h>
#include <stdlib.h>
#include <strings.h>
#include <math.h>
#include <X11/Xlib.h>
#include <X11/Xutil.h>


#define DEBUG 0
#define FALSE 0
#define TRUE 1
#define EPS 2.2250738585072014E-308
#define ABS(x) ((x) < 0 ? -(x) : (x))


typedef struct {
  double x, y;
} point2d;

typedef struct {
  double x, y, z;
} point3d;

typedef struct {
  double a, b, c, d;
} plane3d;

typedef struct {
  double Ncx; /* [sel]     Number of sensor elements in camera's x direction */
  double Nfx; /* [pix]     Number of pixels in frame grabber's x direction   */
  double dx;  /* [mm/sel]  X dimension of camera's sensor element (in mm)    */
  double dy;  /* [mm/sel]  Y dimension of camera's sensor element (in mm)    */
  double dpx; /* [mm/pix]  Effective X dimension of pixel in frame grabber   */
  double dpy; /* [mm/pix]  Effective Y dimension of pixel in frame grabber   */
  double Cx;  /* [pix]     Z axis intercept of camera coordinate system      */
  double Cy;  /* [pix]     Z axis intercept of camera coordinate system      */
  double sx;  /* []        Scale factor to compensate for any error in dpx   */
} camera_parameters;

typedef struct {
  double f;                /* [mm]          */
  double kappa1;           /* [1/mm^2]      */
  double p1;               /* [1/mm]        */
  double p2;               /* [1/mm]        */
  double Tx;               /* [mm]          */
  double Ty;               /* [mm]          */
  double Tz;               /* [mm]          */
  double Rx;               /* [rad]         */
  double Ry;               /* [rad]         */
  double Rz;               /* [rad]         */
  double r1;               /* []            */
  double r2;               /* []            */
  double r3;               /* []            */
  double r4;               /* []            */
  double r5;               /* []            */
  double r6;               /* []            */
  double r7;               /* []            */
  double r8;               /* []            */
  double r9;               /* []            */
} calibration_constants;

typedef struct {

  Display *xdisplay;
  Window xwindow;
  GC xgc;
  Atom xatom;
  XEvent xevent;
  XSizeHints xhints;
  XVisualInfo xvinfo;
  XImage *ximage;

  int RGB;
  int width, height;
  char *name;

} XStuff;


int N, clicks, pheight;
char **images;
point2d *stbs;
camera_parameters cparam;
calibration_constants ccalib;

void CalculateLocation( void );


/** LIGHT CALIBRATION FUNCTIONS */

/*
  Calculate the line segment PaPb that is the shortest route between
  two lines P1P2 and P3P4. Calculate also the values of mua and mub where
     Pa = P1 + mua (P2 - P1)
     Pb = P3 + mub (P4 - P3)
  Return FALSE if no solution exists.
*/
int LineLineIntersect( point3d p1, point3d p2, point3d p3, point3d p4,
		       point3d *pa, point3d *pb, double *mua, double *mub ){

  point3d p13, p43, p21;
  double d1343, d4321, d1321, d4343, d2121;
  double numer, denom;

  p13.x = p1.x - p3.x;
  p13.y = p1.y - p3.y;
  p13.z = p1.z - p3.z;
  p43.x = p4.x - p3.x;
  p43.y = p4.y - p3.y;
  p43.z = p4.z - p3.z;
  if( ABS(p43.x) < EPS && ABS(p43.y) < EPS && ABS(p43.z) < EPS ) return FALSE;

  p21.x = p2.x - p1.x;
  p21.y = p2.y - p1.y;
  p21.z = p2.z - p1.z;
  if( ABS(p21.x) < EPS && ABS(p21.y) < EPS && ABS(p21.z) < EPS ) return FALSE;

  d1343 = p13.x * p43.x + p13.y * p43.y + p13.z * p43.z;
  d4321 = p43.x * p21.x + p43.y * p21.y + p43.z * p21.z;
  d1321 = p13.x * p21.x + p13.y * p21.y + p13.z * p21.z;
  d4343 = p43.x * p43.x + p43.y * p43.y + p43.z * p43.z;
  d2121 = p21.x * p21.x + p21.y * p21.y + p21.z * p21.z;

  denom = d2121 * d4343 - d4321 * d4321;
  if( ABS(denom) < EPS ) return FALSE;
  numer = d1343 * d4321 - d1321 * d4343;

  *mua = numer / denom;
  *mub = (d1343 + d4321 * (*mua)) / d4343;

  pa->x = p1.x + *mua * p21.x;
  pa->y = p1.y + *mua * p21.y;
  pa->z = p1.z + *mua * p21.z;
  pb->x = p3.x + *mub * p43.x;
  pb->y = p3.y + *mub * p43.y;
  pb->z = p3.z + *mub * p43.z;

  return TRUE;

} /*end LineLineIntersect()*/


/*
  Calculate the point of intersection of plane p and vector v.
  Return FALSE if no solution exists.
*/
int VectorPlaneIntersect( plane3d p, point3d v, point3d *ans ){

  double k, denom;

  denom = p.a * v.x + p.b * v.y + p.c * v.z;
  if( ABS(denom) < EPS ) return FALSE;
  k = p.d / denom;

  ans->x = k * v.x;
  ans->y = k * v.y;
  ans->z = k * v.z;

  return TRUE;

} /*end VectorPlaneIntersect()*/


/*
  Calculate the line along which the light lies.
*/
int findLightLine( point3d desk_normal, double desk_distance,
		   double focal_length, double pencil_height,
		   point2d top_img, point2d bottom_img,
		   point3d *shadow_top, point3d *pencil_top ){

  plane3d desk;
  point3d shadow_bottom, temp;
  
  /* find the parameters for the desktop plane */
  desk.a = desk_normal.x;
  desk.b = desk_normal.y;
  desk.c = desk_normal.z;
  desk.d = desk_distance / sqrt( pow(desk_normal.x, 2) +
				 pow(desk_normal.y, 2) +
				 pow(desk_normal.z, 2) );
  
  /* find the 3D point corresponding to the top pencil shadow point */
  temp.x = cparam.dpx * (top_img.x - cparam.Cx);
  temp.y = cparam.dpy * (top_img.y - cparam.Cy);
  temp.z = focal_length;
  VectorPlaneIntersect(desk, temp, shadow_top);

  /* find the 3D point corresponding to the bottom pencil shadow point */
  temp.x = cparam.dpx * (bottom_img.x - cparam.Cx);
  temp.y = cparam.dpy * (bottom_img.y - cparam.Cy);
  temp.z = focal_length;
  VectorPlaneIntersect(desk, temp, &shadow_bottom);

  /* infer the 3D point corresponding to the top of the actual pencil */
  pencil_top->x = shadow_bottom.x + pencil_height * desk_normal.x;
  pencil_top->y = shadow_bottom.y + pencil_height * desk_normal.y;
  pencil_top->z = shadow_bottom.z + pencil_height * desk_normal.z;

  return TRUE;

} /*end findLightLine()*/


/*
  Calculate the point at which the light lies, using two lines.
*/
int findLightPoint( point3d shadow_top_a, point3d pencil_top_a,
		    point3d shadow_top_b, point3d pencil_top_b,
		    point3d *light ){

  point3d end_a, end_b;
  double mu_a, mu_b;

  LineLineIntersect( shadow_top_a, pencil_top_a, shadow_top_b, pencil_top_b,
		     &end_a, &end_b, &mu_a, &mu_b );

  /* light is the mid-point of the shortest line connecting lines a and b */
  light->x = (end_a.x + end_b.x) / 2.0;
  light->y = (end_a.y + end_b.y) / 2.0;
  light->z = (end_a.z + end_b.z) / 2.0;

  return TRUE;

} /*end findLightPoint()*/


int readParams( char *fname,
		camera_parameters *cp, calibration_constants *cc ){

  FILE *fp;
  double sa, ca, sb, cb, sg, cg;

  if( !fname || (fp = fopen(fname, "r")) == 0 ) return FALSE;

  fscanf(fp, "%lf", &(cp->Ncx));
  fscanf(fp, "%lf", &(cp->Nfx));
  fscanf(fp, "%lf", &(cp->dx));
  fscanf(fp, "%lf", &(cp->dy));
  fscanf(fp, "%lf", &(cp->dpx));
  fscanf(fp, "%lf", &(cp->dpy));
  fscanf(fp, "%lf", &(cp->Cx));
  fscanf(fp, "%lf", &(cp->Cy));
  fscanf(fp, "%lf", &(cp->sx));

  fscanf(fp, "%lf", &(cc->f));
  fscanf(fp, "%lf", &(cc->kappa1));
  fscanf(fp, "%lf", &(cc->Tx));
  fscanf(fp, "%lf", &(cc->Ty));
  fscanf(fp, "%lf", &(cc->Tz));
  fscanf(fp, "%lf", &(cc->Rx));
  fscanf(fp, "%lf", &(cc->Ry));
  fscanf(fp, "%lf", &(cc->Rz));

  sa = sin(cc->Rx);
  ca = cos(cc->Rx);
  sb = sin(cc->Ry);
  cb = cos(cc->Ry);
  sg = sin(cc->Rz);
  cg = cos(cc->Rz);

  cc->r1 = cb * cg;
  cc->r2 = cg * sa * sb - ca * sg;
  cc->r3 = sa * sg + ca * cg * sb;
  cc->r4 = cb * sg;
  cc->r5 = sa * sb * sg + ca * cg;
  cc->r6 = ca * sb * sg - cg * sa;
  cc->r7 = -sb;
  cc->r8 = cb * sa;
  cc->r9 = ca * cb;

  fscanf(fp, "%lf", &(cc->p1));
  fscanf(fp, "%lf", &(cc->p2));

  fclose(fp);

  return TRUE;

} /*end readParams()*/


/** X11 FUNCTIONS */

int SetupX( int width, int height, char *name, XStuff *x ){

  x->xdisplay = XOpenDisplay( NULL );
  if( !x->xdisplay ){
    fprintf(stderr, "SetupX(): Can't open display!\n");
    return FALSE;
  }

  if( !XMatchVisualInfo( x->xdisplay, DefaultScreen( x->xdisplay ),
			 24, TrueColor, &x->xvinfo ) ){

    fprintf(stderr, "SetupX(): Requires 24 bit TrueColor display!\n");
    return FALSE;

  } else {

    if( x->xvinfo.blue_mask == 255 ) x->RGB = 1;
    else if( x->xvinfo.red_mask == 255 ) x->RGB = 0;
    else {
      fprintf(stderr, "SetupX(): Can't determine RGB color order!\n");
      return FALSE;
    }

  }

  x->width = width;
  x->height = height;

  x->xwindow = XCreateSimpleWindow( x->xdisplay,
				    DefaultRootWindow(x->xdisplay), 0, 0,
				    x->width, x->height, 0, 0, 0 );
  if( !x->xwindow ){
    fprintf(stderr, "SetupX(): Can't create window!\n");
    return FALSE;
  }

  x->name = name;
  x->xhints.flags = 0;

  XSetStandardProperties( x->xdisplay, x->xwindow, x->name, x->name,
			  0, 0, 0, &x->xhints );

  if( (x->xatom = XInternAtom( x->xdisplay, "WM_DELETE_WINDOW", False )) )
    XSetWMProtocols( x->xdisplay, x->xwindow, &x->xatom, 1 );

  XSelectInput( x->xdisplay, x->xwindow, ExposureMask | ResizeRedirectMask |
		ButtonPressMask | KeyPressMask );

  XMapWindow( x->xdisplay, x->xwindow );

  do XNextEvent( x->xdisplay, &x->xevent );
  while( x->xevent.xany.type != Expose );

  x->xgc = XCreateGC( x->xdisplay, x->xwindow, 0, 0 );

  x->ximage = XGetImage( x->xdisplay, DefaultRootWindow( x->xdisplay ),
			 0, 0, x->width, x->height, AllPlanes, ZPixmap );

  return TRUE;

} /*end SetupX()*/


unsigned long XRGBToPixel( unsigned char R, unsigned char G, unsigned char B,
			   XStuff *x ){

  unsigned long P = 0;

  if( x->RGB )  P = (65536 * R) + (256 * G) + (1 * B);
  else          P = (65536 * B) + (256 * B) + (1 * R);

  return P;

} /*end XRGBToPixel()*/


int XReadImageRaw( char *fname, XStuff *x ){

  FILE *input;
  char line[16384];
  int color;
  int levels;
  int i, j;

  if( !fname || (input = fopen(fname, "r")) == 0 ) return FALSE;

  fread(line, 1, 3, input);

  if( !strncmp(line, "P5\n", 3) ) color = 0;
  else if( !strncmp(line, "P6\n", 3) ) color = 1;
  else {
    fclose(input);
    return FALSE;
  }

  do fgets(line, sizeof line, input);
  while( *line == '#' );

  sscanf(line, "%d %d\n", &x->width, &x->height);

  fgets(line, sizeof line, input);
  sscanf(line, "%d\n", &levels);

  if( !color ){

    for( i = 0; i < x->height; i++ ){
      for( j = 0; j < x->width; j++ ){

	int byte;

	byte = fgetc(input);
	if( byte == EOF ){
	  fclose(input);
	  return FALSE;
	}

	XPutPixel( x->ximage, j, i, XRGBToPixel(byte, byte, byte, x) );

      }
    }

  } else {

    for( i = 0; i < x->height; i++ ){
      for( j = 0; j < x->width; j++ ){

	int byteR, byteG, byteB;

	byteR = fgetc(input);
	if( byteR == EOF ){
	  fclose(input);
	  return FALSE;
	}

	byteG = fgetc(input);
	if( byteG == EOF ){
	  fclose(input);
	  return FALSE;
	}

	byteB = fgetc(input);
	if( byteB == EOF ){
	  fclose(input);
	  return FALSE;
	}

	XPutPixel( x->ximage, j, i, XRGBToPixel(byteR, byteG, byteB, x) );

      }
    }

  }

  fclose(input);

  return TRUE;

} /*end XReadImageRaw()*/


int XRepaint( XStuff *x ){

  XPutImage( x->xdisplay, x->xwindow, x->xgc, x->ximage, 0, 0, 0, 0,
	     x->width, x->height );

  return TRUE;

} /*end XRepaint()*/


int GetImageSize( char *fname, int *width, int *height ){

  FILE *input;
  char line[128];

  if( !fname || (input = fopen(fname, "r")) == 0 ) return FALSE;

  fread(line, 1, 3, input);

  do fgets(line, sizeof line, input);
  while( *line == '#' );

  sscanf(line, "%d %d\n", width, height);

  if( DEBUG ) fprintf(stderr, "GetImageSize(): %d x %d\n", *width, *height);

  return TRUE;

} /*end GetImageSize()*/


void XExit( XStuff *x ){

  XDestroyImage( x->ximage );
  XCloseDisplay( x->xdisplay );

  free(images);
  free(stbs);

  exit(0);

} /*end XExit()*/


void HandleXEvents( XStuff *x ){

  XNextEvent( x->xdisplay, &x->xevent );

  if( x->xevent.xany.type == ClientMessage &&
      ((XClientMessageEvent *) & x->xevent)->data.l[0] == x->xatom ){

    XExit( x );

  }

  switch( x->xevent.type ){

  case Expose:
    XRepaint( x );
    break;

  case ResizeRequest:
    XResizeWindow( x->xdisplay, x->xwindow, x->width, x->height );
    break;

  case ButtonPress:
    clicks++;

    stbs[clicks - 1].x = x->xevent.xbutton.x;
    stbs[clicks - 1].y = x->xevent.xbutton.y;

    if( clicks == N * 2 ){
      CalculateLocation();
      XExit( x );
    }

    if( clicks % 2 == 0 ){
      XReadImageRaw( images[clicks / 2], x );
      XRepaint( x );
    }
    break;

  case KeyPress:
    break;

  default:
    break;

  }

} /*end HandleXEvents()*/


void CalculateLocation( void ){

  int i, j, l;
  double dist;
  point3d norm, lite, *tsps, *lcs;

  tsps = (point3d *) malloc( 2 * N * sizeof(point3d) );
  lcs = (point3d *) malloc( N * (N - 1) / 2 * sizeof(point3d) );

  /* last column of R from tsai-v3.0b3 */
  norm.x = ccalib.r3;
  norm.y = ccalib.r6;
  norm.z = ccalib.r9;

  /* dot product of the translation and normal vectors */
  dist = ccalib.Tx * norm.x + ccalib.Ty * norm.y + ccalib.Tz * norm.z;

  if( DEBUG ) fprintf(stderr, "(%.2f, %.2f, %.2f) & %.2f\n",
		      norm.x, norm.y, norm.z, dist);

  for( i = 0; i < N * 2; i += 2 ){

    findLightLine( norm, dist, ccalib.f, pheight, stbs[i], stbs[i + 1],
		   &tsps[i], &tsps[i + 1] );

    if( DEBUG ) fprintf(stderr, "(%.2f, %.2f, %.2f) & (%.2f, %.2f, %.2f)\n",
			tsps[i].x, tsps[i].y, tsps[i].z,
			tsps[i + 1].x, tsps[i + 1].y, tsps[i + 1].z);
  }

  l = 0;
  for( i = 0; i < N; i++ ){
    for( j = i + 1; j < N; j++ ){

      findLightPoint( tsps[i * 2], tsps[i * 2 + 1],
		      tsps[j * 2], tsps[j * 2 + 1],
		      &lcs[l++]);

    }
  }

  if( DEBUG ) fprintf(stderr, "%d\n", l);

  lite.x = 0.0;
  lite.y = 0.0;
  lite.z = 0.0;
  for( i = 0; i < l; i++ ){

    if( DEBUG ) fprintf(stderr, "(%.2f, %.2f, %.2f)\n",
			lcs[i].x, lcs[i].y, lcs[i].z);

    lite.x += lcs[i].x;
    lite.y += lcs[i].y;
    lite.z += lcs[i].z;
  }
  lite.x /= (double)l;
  lite.y /= (double)l;
  lite.z /= (double)l;

  if( DEBUG ) fprintf(stderr, "(%.2f, %.2f, %.2f)\n", lite.x, lite.y, lite.z);

  printf("%17.10e %17.10e %17.10e\n", lite.x, lite.y, lite.z);

  free(tsps);
  free(lcs);

  return;

} /*end CalculateLocation()*/


int main( int argc, char **argv ){

  int w, h, i;
  XStuff xinst;

  if( argc < 5 || atoi(argv[2]) <= 0 ){
    fprintf(stderr, "Usage: %s tsai_params height_mm images...\n", argv[0]);
    return -1;
  }

  if( !GetImageSize( argv[3], &w, &h ) ) return -1;

  SetupX( w, h, argv[0], &xinst );

  readParams( argv[1], &cparam, &ccalib );

  N = argc - 3;
  clicks = 0;
  pheight = atoi(argv[2]);

  images = (char **) malloc( N * sizeof(char *) );
  stbs = (point2d *) malloc( 2 * N * sizeof(point2d) );

  for( i = 0; i < N; i++ ){
    images[i] = argv[3+i];
  }

  XReadImageRaw( images[0], &xinst );
  XRepaint( &xinst );

  while( TRUE ){
    HandleXEvents( &xinst );
  }

  return 0;

} /*end main()*/
