#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <model.h>
#include <math.h>

#define LNLEN		1000
#define Number(x)	(sizeof(x) / sizeof(x[0]))

/****************************************************************
 * Types
 ****************************************************************/

struct camera_parameters {
    double    Ncx;
    double    Nfx;
    double    dx;
    double    dy;
    double    dpx;
    double    dpy;
    double    Cx;
    double    Cy;
    double    sx;
};

struct calibration_constants {
    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;
};

typedef double matrix4x4 [4 /*row*/] [4 /*col*/];

typedef struct {
  float x, y, z, w;
} coordType;

char *progname;
char *filename;
int linenum, verbose = 0;

/****************************************************************
 * Prototypes
 ****************************************************************/

void main(int argc, char **argv);
void transformPoints(matrix4x4 *xform);
void transformPoint(coordType *imagePt, coordType *worldPt, matrix4x4 *xform);
void getTransformation(char *filename, matrix4x4 *xform);
void readCpcc(char *filename, struct camera_parameters *cp, struct calibration_constants *cc);
void buildRotationMatrix(struct calibration_constants *cc);
matrix4x4 *getRotationMatrix(struct calibration_constants *cc);
matrix4x4 *buildIntrinsicMatrix(struct camera_parameters *cp, struct calibration_constants *cc);
matrix4x4 *buildExtrinsicMatrix(struct calibration_constants *cc);
void matrix4x4Print(FILE *fp, matrix4x4 *m);
void multMbyPoint(coordType *result, matrix4x4 *m, coordType *pt);
void multMbyM(matrix4x4 *result, matrix4x4 *m0, matrix4x4 *m1);
FILE *myFopen(char *name, char *mode);
void error(char *fmt, ...);

/****************************************************************
 * Main
 ****************************************************************/

void
main (int argc, char **argv)
{
  matrix4x4 xform;

  progname = argv [0];
  if (argc >= 2 && 0 == strcmp (argv [1], "-v")) {
    verbose = 1;
    --argc;
    ++argv;
  }
  if (argc != 2) {
    fprintf (stderr, "Usage: %s [-v] <cpcc file> [<cor file>]\n", progname);
    exit (-1);
  }
  getTransformation (argv [1], &xform);
  transformPoints (&xform);
  exit (0);
}

/****************************************************************
 * Transform points
 ****************************************************************/
  
/* Prompt the user for a 3D world point, transform it into an
 * image point, print the image coordinates.
 */
void
transformPoints (matrix4x4 *xform)
{
  char s [LNLEN];
  coordType worldPt, xformPt;
  double x, y, z;

  while (1) {
    printf ("3D world point? ");
    gets (s);
    if (3 != sscanf (s, "%le %le %e", &x, &y, &z))
      return;
    worldPt.x = x;
    worldPt.y = y;
    worldPt.z = z;
    worldPt.w = 1;
    transformPoint (&xformPt, &worldPt, xform);
    printf ("image coordinates (%lg, %lg)\n", xformPt.x, xformPt.y);
  }
}

/* Given a 3D world point and a transformation matrix,
 * transform it into a 2D image point.
 */
void
transformPoint (coordType *imagePt, coordType *worldPt, matrix4x4 *xform)
{
  multMbyPoint (imagePt, xform, worldPt);
  imagePt->x /= imagePt->z;
  imagePt->y /= imagePt->z;
}

/****************************************************************
 * Get transformation matrix
 ****************************************************************/

void
getTransformation (char *filename, matrix4x4 *xform)
{
  struct camera_parameters cp;
  struct calibration_constants cc;
  matrix4x4 *rot, *intrinsic, *extrinsic;

  readCpcc (filename, &cp, &cc);
  buildRotationMatrix (&cc);
  intrinsic = buildIntrinsicMatrix (&cp, &cc);
  extrinsic = buildExtrinsicMatrix (&cc);
  multMbyM (xform, intrinsic, extrinsic);
  if (verbose) {
    rot = getRotationMatrix (&cc);
    printf ("Rotation matrix:\n");
    matrix4x4Print (stdout, rot);

    printf ("Intrinsic matrix:\n");
    matrix4x4Print (stdout, intrinsic);

    printf ("Extrinsic matrix:\n");
    matrix4x4Print (stdout, extrinsic);

    printf ("Transformation matrix:\n");
    matrix4x4Print (stdout, xform);
  }
}

/* Read a Tsai format .cpcc file into a cp and cc struct.
 */
void
readCpcc (char *filename, struct camera_parameters *cp,
  struct calibration_constants *cc)
{
  FILE *fp;

  fp = myFopen (filename, "r");

  /* Read intrinsic and extrinsic parameters from .cpcc file */
  fscanf (fp, "%lf", &(cp->Ncx));	/* 0 */ /* not used */
  fscanf (fp, "%lf", &(cp->Nfx));	/* 1 */ /* not used */
  fscanf (fp, "%lf", &(cp->dx));	/* 2 */ /* not used */
  fscanf (fp, "%lf", &(cp->dy));	/* 3 */ /* not used */
  fscanf (fp, "%lf", &(cp->dpx));	/* 4 */ 
  fscanf (fp, "%lf", &(cp->dpy));	/* 5 */ 
  fscanf (fp, "%lf", &(cp->Cx));	/* 6 */ 
  fscanf (fp, "%lf", &(cp->Cy));	/* 7 */ 
  fscanf (fp, "%lf", &(cp->sx));	/* 8 */ 

  fscanf (fp, "%lf", &(cc->f));		/* 9 */ 
  fscanf (fp, "%lf", &(cc->kappa1));	/* 10 */ 
  fscanf (fp, "%lf", &(cc->Tx));	/* 11*/ 
  fscanf (fp, "%lf", &(cc->Ty));	/* 12 */ 
  fscanf (fp, "%lf", &(cc->Tz));	/* 13 */ 
  fscanf (fp, "%lf", &(cc->Rx));	/* 14 */ 
  fscanf (fp, "%lf", &(cc->Ry));	/* 15 */ 
  fscanf (fp, "%lf", &(cc->Rz));	/* 16 */ 

  fclose (fp);
}

/* Given a Tsai cc struct, compute the corresponding rotation matrix.
 * Put result in r* fields of cc struct.
 */
void
buildRotationMatrix (struct calibration_constants *cc)
{
  double sinRx, cosRx;
  double sinRy, cosRy;
  double sinRz, cosRz;

  sinRx = sin (cc->Rx);
  cosRx = cos (cc->Rx);
  sinRy = sin (cc->Ry);
  cosRy = cos (cc->Ry);
  sinRz = sin (cc->Rz);
  cosRz = cos (cc->Rz);

  /* compute rotation matrix (Rz * Ry * Rx) */
  cc->r1 = cosRy * cosRz;
  cc->r2 = cosRz * sinRx * sinRy - cosRx * sinRz;
  cc->r3 = sinRx * sinRz + cosRx * cosRz * sinRy;
  cc->r4 = cosRy * sinRz;
  cc->r5 = sinRx * sinRy * sinRz + cosRx * cosRz;
  cc->r6 = cosRx * sinRy * sinRz - cosRz * sinRx;
  cc->r7 = -sinRy;
  cc->r8 = cosRy * sinRx;
  cc->r9 = cosRx * cosRy;
}

/* Read rotation matrix out of Tsai cc struct, into
 * a 4x4 C matrix.
 */
matrix4x4 *
getRotationMatrix (struct calibration_constants *cc)
{
  static matrix4x4 rot;

  rot [0] [0] = cc->r1;
  rot [0] [1] = cc->r2;
  rot [0] [2] = cc->r3;
  rot [0] [3] = 0;

  rot [1] [0] = cc->r4;
  rot [1] [1] = cc->r5;
  rot [1] [2] = cc->r6;
  rot [1] [3] = 0;

  rot [2] [0] = cc->r7;
  rot [2] [1] = cc->r8;
  rot [2] [2] = cc->r9;
  rot [2] [3] = 0;

  rot [3] [0] = 0;
  rot [3] [1] = 0;
  rot [3] [2] = 0;
  rot [3] [3] = 0;

  return &rot;
}

/* Build an intrinsic camera transformation using camera center,
 * pixel skew and pixel dimensions from cc and cp structs.
 */
matrix4x4 *
buildIntrinsicMatrix (struct camera_parameters *cp,
  struct calibration_constants *cc)
{
  static matrix4x4 intrinsic = { /* a 3x4 matrix really */
	{1, 0, 0, 0},
	{0, 1, 0, 0},
	{0, 0, 1, 0},
	{0, 0, 0, 1}};

  intrinsic[0][0] = cc->f*cp->sx/cp->dpx;
  intrinsic[1][1] = cc->f/cp->dpy;
  intrinsic[0][2] = cp->Cx;
  intrinsic[1][2] = cp->Cy;
  return &intrinsic;
}

/* Build an extrinsic camera transformation using rotation and
 * translation info cc struct.
 */
matrix4x4 *
buildExtrinsicMatrix (struct calibration_constants *cc)
{
  static matrix4x4 extrinsic = {
	{1, 0, 0, 0},
	{0, 1, 0, 0},
	{0, 0, 1, 0},
	{0, 0, 0, 1}};

  extrinsic[0][0] = cc->r1;
  extrinsic[0][1] = cc->r2;
  extrinsic[0][2] = cc->r3;
  extrinsic[0][3] = cc->Tx;

  extrinsic[1][0] = cc->r4;
  extrinsic[1][1] = cc->r5;
  extrinsic[1][2] = cc->r6;
  extrinsic[1][3] = cc->Ty;

  extrinsic[2][0] = cc->r7;
  extrinsic[2][1] = cc->r8;
  extrinsic[2][2] = cc->r9;
  extrinsic[2][3] = cc->Tz;

  return &extrinsic;
}

/****************************************************************
 * Matrix operations
 ****************************************************************/

/* Print a 4x4 matrix
 */
void
matrix4x4Print (FILE *fp, matrix4x4 *m)
{
  int i,j;

  for (i = 0; i < 4; i++) {
    for (j = 0;j<4;j++) 
      fprintf (fp, "%15f ", (*m)[i][j]);
    fprintf (fp, "\n");
  }
}

/* Multiply a 4x4 matrix times a coordType (a 4x1 vector).
 * This should be fast because the array indexing is constant
 * at compile time.
 */
void
multMbyPoint (coordType *result, matrix4x4 *m, coordType *pt)
{
  result->x =
    (*m)[0][0] * pt->x +
    (*m)[0][1] * pt->y +
    (*m)[0][2] * pt->z +
    (*m)[0][3] * pt->w;

  result->y =
    (*m)[1][0] * pt->x +
    (*m)[1][1] * pt->y +
    (*m)[1][2] * pt->z +
    (*m)[1][3] * pt->w;

  result->z =
    (*m)[2][0] * pt->x +
    (*m)[2][1] * pt->y +
    (*m)[2][2] * pt->z +
    (*m)[2][3] * pt->w;

  result->w =
    (*m)[3][0] * pt->x +
    (*m)[3][1] * pt->y +
    (*m)[3][2] * pt->z +
    (*m)[3][3] * pt->w;
}

/* Multiply a 4x4 matrix (m0) times a 4x4 matrix (m1).
 * This should be fast because the array indexing is constant
 * at compile time.
 */
void
multMbyM (matrix4x4 *result, matrix4x4 *m0, matrix4x4 *m1)
{
  (*result)[0][0] =
    (*m0)[0][0] * (*m1)[0][0] +
    (*m0)[0][1] * (*m1)[1][0] +
    (*m0)[0][2] * (*m1)[2][0] +
    (*m0)[0][3] * (*m1)[3][0];
  (*result)[0][1] =
    (*m0)[0][0] * (*m1)[0][1] +
    (*m0)[0][1] * (*m1)[1][1] +
    (*m0)[0][2] * (*m1)[2][1] +
    (*m0)[0][3] * (*m1)[3][1];
  (*result)[0][2] =
    (*m0)[0][0] * (*m1)[0][2] +
    (*m0)[0][1] * (*m1)[1][2] +
    (*m0)[0][2] * (*m1)[2][2] +
    (*m0)[0][3] * (*m1)[3][2];
  (*result)[0][3] =
    (*m0)[0][0] * (*m1)[0][3] +
    (*m0)[0][1] * (*m1)[1][3] +
    (*m0)[0][2] * (*m1)[2][3] +
    (*m0)[0][3] * (*m1)[3][3];
  (*result)[1][0] =
    (*m0)[1][0] * (*m1)[0][0] +
    (*m0)[1][1] * (*m1)[1][0] +
    (*m0)[1][2] * (*m1)[2][0] +
    (*m0)[1][3] * (*m1)[3][0];
  (*result)[1][1] =
    (*m0)[1][0] * (*m1)[0][1] +
    (*m0)[1][1] * (*m1)[1][1] +
    (*m0)[1][2] * (*m1)[2][1] +
    (*m0)[1][3] * (*m1)[3][1];
  (*result)[1][2] =
    (*m0)[1][0] * (*m1)[0][2] +
    (*m0)[1][1] * (*m1)[1][2] +
    (*m0)[1][2] * (*m1)[2][2] +
    (*m0)[1][3] * (*m1)[3][2];
  (*result)[1][3] =
    (*m0)[1][0] * (*m1)[0][3] +
    (*m0)[1][1] * (*m1)[1][3] +
    (*m0)[1][2] * (*m1)[2][3] +
    (*m0)[1][3] * (*m1)[3][3];
  (*result)[2][0] =
    (*m0)[2][0] * (*m1)[0][0] +
    (*m0)[2][1] * (*m1)[1][0] +
    (*m0)[2][2] * (*m1)[2][0] +
    (*m0)[2][3] * (*m1)[3][0];
  (*result)[2][1] =
    (*m0)[2][0] * (*m1)[0][1] +
    (*m0)[2][1] * (*m1)[1][1] +
    (*m0)[2][2] * (*m1)[2][1] +
    (*m0)[2][3] * (*m1)[3][1];
  (*result)[2][2] =
    (*m0)[2][0] * (*m1)[0][2] +
    (*m0)[2][1] * (*m1)[1][2] +
    (*m0)[2][2] * (*m1)[2][2] +
    (*m0)[2][3] * (*m1)[3][2];
  (*result)[2][3] =
    (*m0)[2][0] * (*m1)[0][3] +
    (*m0)[2][1] * (*m1)[1][3] +
    (*m0)[2][2] * (*m1)[2][3] +
    (*m0)[2][3] * (*m1)[3][3];
  (*result)[3][0] =
    (*m0)[3][0] * (*m1)[0][0] +
    (*m0)[3][1] * (*m1)[1][0] +
    (*m0)[3][2] * (*m1)[2][0] +
    (*m0)[3][3] * (*m1)[3][0];
  (*result)[3][1] =
    (*m0)[3][0] * (*m1)[0][1] +
    (*m0)[3][1] * (*m1)[1][1] +
    (*m0)[3][2] * (*m1)[2][1] +
    (*m0)[3][3] * (*m1)[3][1];
  (*result)[3][2] =
    (*m0)[3][0] * (*m1)[0][2] +
    (*m0)[3][1] * (*m1)[1][2] +
    (*m0)[3][2] * (*m1)[2][2] +
    (*m0)[3][3] * (*m1)[3][2];
  (*result)[3][3] =
    (*m0)[3][0] * (*m1)[0][3] +
    (*m0)[3][1] * (*m1)[1][3] +
    (*m0)[3][2] * (*m1)[2][3] +
    (*m0)[3][3] * (*m1)[3][3];
}

/****************************************************************
 * Utilities
 ****************************************************************/

/* Open a file; on error, print a message and die.
 */
FILE
*myFopen (char *name, char *mode)
{
  FILE *f;

  if (NULL == (f = fopen (name, mode)))
    error ("!cannot open %s, quitting", name);
  return f;
}

/* Print an error message printf-style.  If the message begins with
 * a !, die after printing.  If  the message begins with a #, print
 * filename and linenum.
 */
void
error (char *fmt, ...)
{
  int fatal = 0, printFile = 0;
  va_list args;

  va_start (args, fmt);
  while (*fmt == '!' || *fmt == '#') {
    if (*fmt == '!')
      fatal = 1;
    else
      printFile = 1;
    ++fmt;
  }
  fprintf (stderr, "%s: ", progname);
  if (printFile)
    fprintf (stderr, "%s(%d): ", filename, linenum);
  vfprintf (stderr, fmt, args);
  va_end (args);
  fprintf (stderr, "\n");
  if (fatal) {
    exit (-1);
  } else
    fflush (stderr);
}