#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;



/* Given a point in world coordinates and a projection
 * matrix, find the point in the image that the world point
 * projects to.
 */
void
transformPoint (coordType *imagePt, coordType *worldPt, matrix4x4 *xform)
{
  multMbyPoint (imagePt, xform, worldPt);
  imagePt->x /= imagePt->z;
  imagePt->y /= imagePt->z;
}

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

/* Given the file name of a .cpcc file, build the projection
 * 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);
}

void
readCpcc (char *filename, struct camera_parameters *cp,
  struct calibration_constants *cc)
{
  FILE *fp;

  fp = fopen (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);
}

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;
}

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;
}

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;
}

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
 ****************************************************************/

/* 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];
}