#include <math.h>

#ifndef SWAP
#define SWAP(a,b) {(temp)=(a);(a)=(b);(b)=(temp);}
#endif

#ifndef SQR
#define SQR(a) ((a)*(a))
#endif

#ifndef ABS
#define ABS(a) (((a)<0)?-(a):(a))
#endif

#define TOL 1e-10

#define MAXS 137
#define MAX_POINTS 500

static double X[MAXS], M[MAXS*MAXS];

static int Gauss (double a[MAXS*MAXS], int n, double b[MAXS])
{
 int    i, j, k;
 int    imax;
 double amax, rmax, temp;

 for (j=0; j<n-1; j++) {
  rmax = 0.0;
  imax = j;
  for (i=j; i<n; i++) {
   amax = 0.0;
   for (k=j; k<n; k++)
    if (ABS(a[n*i+k]) > amax)
     amax = ABS(a[n*i+k]);
   if (amax < TOL) return 0;
   else if ((ABS(a[n*i+j]) > rmax*amax) && (ABS(a[n*i+j]) >= TOL)) {
    rmax = ABS(a[n*i+j]) / amax;
    imax = i;
   }
  }
  if (ABS(a[n*imax+j])<TOL) {
   for (k=imax+1; k<n; k++)
    if (ABS(a[n*k+j]) < TOL)
     imax = k;
   if (ABS(a[n*imax+j]) < TOL)
    return 0;
  }
  if (imax != j) {
   for (k=j; k<n; k++)
    SWAP(a[n*imax+k], a[n*j+k]);
   SWAP(b[imax], b[j]);
  }
  for (i=j+1; i<n; i++) {
   double aux = a[n*i+j] / a[n*j+j];
   for (k=j+1; k<n; k++)
    a[n*i+k] -= aux*a[n*j+k];
   b[i] -= aux*b[j];
  }
 }
 if (ABS(a[n*(n-1)+n-1]) <= TOL)
  return 0;
 else {
  b[n-1] /= a[n*(n-1)+n-1];
  for (i=n-2; i>=0; i--) {
   for (j=i+1; j<n; j++)
    b[i] -= a[n*i+j]*b[j];
   b[i] /= a[n*i+i];
  }
 }
 return 1;
}

#define SIGNBIT(a)      (((a) > 0) ? 0 : 1)

static double U[7];

static int MinQuad(int nl, int nc, double A[MAX_POINTS][7], double B[MAX_POINTS])
{
 int i, j, k;

 for (i=0; i<nc; i++)
  for (j=0; j<nc; j++) {
   M[nc*i+j] = A[0][j]*A[0][i];
   for (k=1; k<nl; k++)
    M[nc*i+j] += A[k][j]*A[k][i];
  }

  for (i=0; i<nc; i++) {
   X[i] = A[0][i]*B[0];
   for (k=1; k<nl; k++)
    X[i] += A[k][i]*B[k];
  }

  if (!Gauss(M, nc, X) != 0) return 0;

  for (i=0; i<nc; i++) 
   B[i] = X[i];
  return 1;
}

static int ncc_compute_U(int np, double *pw, double *pf)
{
 int i;

 double A[MAX_POINTS][7];
 double B[MAX_POINTS];

 for (i=0; i<np; i++) {
  A[i][0] = pf[2*i+1]*pw[3*i];
  A[i][1] = pf[2*i+1]*pw[3*i+1];
  A[i][2] = pf[2*i+1]*pw[3*i+2];
  A[i][3] = pf[2*i+1];
  A[i][4] = -pf[2*i]*pw[3*i];
  A[i][5] = -pf[2*i]*pw[3*i+1];
  A[i][6] = -pf[2*i]*pw[3*i+2];
  B[i] = pf[2*i];
 }

 if (!MinQuad(np, 7, A, B)) return 0;

 U[0] = B[0];
 U[1] = B[1];
 U[2] = B[2];
 U[3] = B[3];
 U[4] = B[4];
 U[5] = B[5];
 U[6] = B[6];

 return 1;
}

#define EPSILON		1.0E-8

static void ncc_compute_Tx_and_Ty(int np, double *pw, double *pf, double T[3])
{
 double Ty_squared, distance, far_distance;
 double Tx, Ty, r1, r2, r3, r4, r5, r6, x, y;
 int i, far_point;

 Ty_squared = 1.0/(SQR(U[4]) + SQR(U[5]) + SQR(U[6]));

 far_distance = 0;
 far_point = 0;
 for (i=0; i<np; i++)
  if ((distance = SQR(pf[2*i]) + SQR(pf[2*i+1])) > far_distance) {
   far_point = i;
   far_distance = distance;
  }

 Ty = sqrt(Ty_squared);
 r1 = U[0]*Ty;
 r2 = U[1]*Ty;
 r3 = U[2]*Ty;
 Tx = U[3]*Ty;
 r4 = U[4]*Ty;
 r5 = U[5]*Ty;
 r6 = U[6]*Ty;
 x = r1*pw[3*far_point] + r2*pw[3*far_point+1] + r3*pw[3*far_point+2] + Tx;
 y = r4*pw[3*far_point] + r5*pw[3*far_point+1] + r6*pw[3*far_point+2] + Ty;

 if ((SIGNBIT (x) != SIGNBIT (pf[2*far_point])) || (SIGNBIT (y) != SIGNBIT (pf[2*far_point+1])))
  Ty = -Ty;

 T[0] = U[3]*Ty;
 T[1] = Ty;
}

/* Some math libraries may not have the sincos() routine */
#ifdef _SINCOS_
void sincos();
#define SINCOS(x,s,c)   sincos(x,&s,&c)
#else
double sin(), cos();
#define SINCOS(x,s,c)   s=sin(x);c=cos(x)
#endif

static void ncc_compute_R(double T[3], double r[9])
{
 double Rx, Ry, Rz;
 double sa, ca, sb, cb, sg, cg;

 r[0] = U[0]*T[1];
 r[1] = U[1]*T[1];
 r[2] = U[2]*T[1];

 r[3] = U[4]*T[1];
 r[4] = U[5]*T[1];
 r[5] = U[6]*T[1];

 r[6] = r[1]*r[5] - r[2]*r[4];
 //r[7] = r[2]*r[3] - r[0]*r[5];
 //r[8] = r[0]*r[4] - r[1]*r[3];

 Rz = atan2(r[3], r[0]);
 SINCOS (Rz, sg, cg);
 Ry = atan2(-r[6], r[0]*cg + r[3]*sg);
 Rx = atan2(r[2]*sg - r[5]*cg, r[4]*cg - r[1]*sg);
 SINCOS (Rx, sa, ca);
 SINCOS (Ry, sb, cb);

 r[0] = cb*cg;
 r[1] = cg*sa*sb - ca*sg;
 r[2] = sa*sg + ca*cg*sb;
 r[3] = cb*sg;
 r[4] = sa*sb*sg + ca*cg;
 r[5] = ca*sb*sg - cg*sa;
 r[6] = -sb;
 r[7] = cb*sa;
 r[8] = ca*cb;
}

static int ncc_compute_approximate_f_and_Tz(int np, double *pw, double *pf, double *f, double T[3], double r[9])
{
 double A[MAX_POINTS][7];
 double B[MAX_POINTS];
 int i;

 for (i=0; i<np; i++) {
  A[i][0] = r[3]*pw[3*i] + r[4]*pw[3*i+1] + r[5]*pw[3*i+2] + T[1];
  A[i][1] = -pf[2*i+1];
  B[i] = (r[6]*pw[3*i] + r[7]*pw[3*i+1] + r[8]*pw[3*i+2])*pf[2*i+1];
 }

 if (!MinQuad(np, 2, A, B)) return 0;

 *f = B[0];
 T[2] = B[1];

 return 1;
}

#define h (1e-6)

static double mrqcof_NCC_tsai_f_Tz_Error(double f, double T[3], int np, double *pw, double *pf, double r[9])
{
 int i;
 double error;
 double xc, yc, zc;
 for (error=0, i=0; i<np; i++) {
  xc = r[0]*pw[3*i] + r[1]*pw[3*i+1] + r[2]*pw[3*i+2] + T[0];
  yc = r[3]*pw[3*i] + r[4]*pw[3*i+1] + r[5]*pw[3*i+2] + T[1];
  zc = r[6]*pw[3*i] + r[7]*pw[3*i+1] + r[8]*pw[3*i+2] + T[2];
  xc = f*xc/zc - pf[2*i];
  yc = f*yc/zc - pf[2*i+1];
  error = xc*xc + yc*yc;
 }

 return error;
}

static double mrqcof_NCC_tsai_f_Tz(double f, double T[3], double alpha[2][2], double beta[2],
                                   int np, double *pw, double *pf, double r[9], double d)
{
 double t0[3], t1[3];
 double den, den2;
 double df, dTz,
        dfdf, dfdTz,
              dTzdTz;

 den = 1.0/(2.0*h);
 den2 = SQR(den);

 df = (mrqcof_NCC_tsai_f_Tz_Error(f+h, T, np, pw, pf, r) - mrqcof_NCC_tsai_f_Tz_Error(f-h, T, np, pw, pf, r))*den;

 t0[0] = T[0]; t0[1] = T[1]; t0[2] = T[2]+h;
 t1[0] = T[0]; t1[1] = T[1]; t1[2] = T[2]-h;
 dTz = (mrqcof_NCC_tsai_f_Tz_Error(f, t0, np, pw, pf, r) - mrqcof_NCC_tsai_f_Tz_Error(f, t1, np, pw, pf, r))*den;

 dfdf = (mrqcof_NCC_tsai_f_Tz_Error(f+2*h, T, np, pw, pf, r) - 2*d + mrqcof_NCC_tsai_f_Tz_Error(f-2*h, T, np, pw, pf, r))*den2;

 t0[0] = T[0]; t0[1] = T[1]; t0[2] = T[2]+2*h;
 t1[0] = T[0]; t1[1] = T[1]; t1[2] = T[2]-2*h;
 dTzdTz = (mrqcof_NCC_tsai_f_Tz_Error(f, t0, np, pw, pf, r) - 2*d + mrqcof_NCC_tsai_f_Tz_Error(f, t1, np, pw, pf, r))*den2;

 t0[0] = T[0]; t0[1] = T[1]; t0[2] = T[2]+h;
 t1[0] = T[0]; t1[1] = T[1]; t1[2] = T[2]-h;
 dfdTz = ((mrqcof_NCC_tsai_f_Tz_Error(f+h, t0, np, pw, pf, r) - mrqcof_NCC_tsai_f_Tz_Error(f+h, t1, np, pw, pf, r)) -
          (mrqcof_NCC_tsai_f_Tz_Error(f-h, t0, np, pw, pf, r) - mrqcof_NCC_tsai_f_Tz_Error(f-h, t1, np, pw, pf, r)))*den2;

 alpha[0][0] =  dfdf; alpha[0][1] =  dfdTz;
 alpha[1][0] = dfdTz; alpha[1][1] = dTzdTz;

 beta[0] = df;
 beta[1] = dTz;

 return d;
}

static double mrqmin_tsai_f_Tz(double *f, double T[3], double *lambda,
                               int np, double *pw, double *pf, double r[9])
{
 int j, k;
 static double oresult;
 static double alpha[2][2], beta[2];
 static double result;
 static double laux;
 double ftry, Ttry[3];

 if (*lambda == -2.0) {
  *lambda = 0.0001;
  oresult = result = mrqcof_NCC_tsai_f_Tz_Error(*f, T, np, pw, pf, r);
  mrqcof_NCC_tsai_f_Tz(*f, T, alpha, beta, np, pw, pf, r, result);
 } else if (*lambda == -1.0) {
  *lambda = laux;
  oresult = result = mrqcof_NCC_tsai_f_Tz_Error(*f, T, np, pw, pf, r);
  mrqcof_NCC_tsai_f_Tz(*f, T, alpha, beta, np, pw, pf, r, result);
 } else if (*lambda == 0.0)	return result;

 for (j=0; j<2; j++) {
  for (k=0; k<2; k++)
   M[2*j+k] = alpha[j][k];
  M[2*j+j] = alpha[j][j]*(1.0+(*lambda));
  X[j] = beta[j];
 }
 Gauss(M, 2, X);

 ftry = *f - X[0];
 Ttry[0] = T[0];
 Ttry[1] = T[1];
 Ttry[2] = T[2] - X[1];

 result = mrqcof_NCC_tsai_f_Tz_Error(ftry, Ttry, np, pw, pf, r);
 if (result < oresult) {
  *lambda *= 0.1;
  oresult = result;
  *f = ftry;
  T[2] = Ttry[2];
  mrqcof_NCC_tsai_f_Tz(*f, T, alpha, beta, np, pw, pf, r, result);
 } else {
  *lambda *= 10;
  result = oresult;
 }
 laux = *lambda;
 return result;
}

static int ncc_compute_exact_f_and_Tz(int np, double *pw, double *pf, double *f, double T[3], double r[9])
{
 double lambda;
 double l, erro0, erro1;

 lambda = -2;
 erro0 = mrqmin_tsai_f_Tz(f, T, &lambda, np, pw, pf, r);

 do {
  l = lambda;
  erro1 = erro0;
  erro0 = mrqmin_tsai_f_Tz(f, T, &lambda, np, pw, pf, r);
 } while (!((ABS(erro1-erro0)<1e-15 && lambda<l) || (lambda<1e-20) || (lambda>1e20)));

 return 1;
}

static int ncc_compute_Tz(int np, double *pw, double *pf, double f, double T[3], double r[9])
{
 double A[MAX_POINTS][7];
 double B[MAX_POINTS];
 int i;

 for (i=0; i<np; i++) {
  A[i][0] = -pf[2*i+1];
  B[i] = (r[6]*pw[3*i] + r[7]*pw[3*i+1] + r[8]*pw[3*i+2])*pf[2*i+1] - f*(r[3]*pw[3*i] + r[4]*pw[3*i+1] + r[5]*pw[3*i+2] + T[1]);
 }

 if (!MinQuad(np, 1, A, B)) return 0;

 T[2] = B[0];

 return 1;
}

int tcgOGLTsai3DCalibration(int np, double *pw, double *pf, double *f, double T[3], double r[9])
{
 if (!ncc_compute_U(np, pw, pf)) return 0;
 ncc_compute_Tx_and_Ty(np, pw, pf, T);
 ncc_compute_R(T, r);
 if (!ncc_compute_approximate_f_and_Tz(np, pw, pf, f, T, r)) return 0;
 if (*f > 0) {
  r[2] = -r[2];
  r[5] = -r[5];
  r[6] = -r[6];
  r[7] = -r[7];
  if (!ncc_compute_approximate_f_and_Tz(np, pw, pf, f, T, r)) return 0;
  if (*f > 0) return 0;
 }
 return ncc_compute_exact_f_and_Tz(np, pw, pf, f, T, r);
}

int tcgOGLTsai3DCalibration_Tr(int np, double *pw, double *pf, double f, double T[3], double r[9])
{
 if (!ncc_compute_U(np, pw, pf)) return 0;
 ncc_compute_Tx_and_Ty(np, pw, pf, T);
 ncc_compute_R(T, r);
 return ncc_compute_Tz(np, pw, pf, f, T, r);
}

void tcgOGLTsai3DProject(double f, double T[3], double r[9], double pwx, double pwy, double pwz, double *pfx, double *pfy)
{
 double xc = r[0]*pwx + r[1]*pwy + r[2]*pwz + T[0];
 double yc = r[3]*pwx + r[4]*pwy + r[5]*pwz + T[1];
 double zc = r[6]*pwx + r[7]*pwy + r[8]*pwz + T[2];

 *pfx = f*xc / zc;
 *pfy = f*yc / zc;
}

void tcgOGLTsai3DUnproject(double f, double T[3], double r[9], double pfx, double pfy, double *pwx, double *pwy, double pwz)
{
 double common_denominator;

 common_denominator = ((r[0]*r[7] - r[1]*r[6])*pfy + (r[4]*r[6] - r[3]*r[7])*pfx + (r[1]*r[3] - r[0]*r[4])*f);

 *pwx = (((r[1]*r[8] - r[2]*r[7])*pfy + (r[5]*r[7] - r[4]*r[8])*pfx + (r[2]*r[4] - r[1]*r[5])*f)*pwz +
         (r[1]*T[2] - r[7]*T[0])*pfy + (r[7]*T[1] - r[4]*T[2])*pfx + (r[4]*T[0] - r[1]*T[1])*f) / common_denominator;

 *pwy = -(((r[0]*r[8] - r[2]*r[6])*pfy + (r[5]*r[6] - r[3]*r[8])*pfx + (r[2]*r[3] - r[0]*r[5])*f)*pwz +
          (r[0]*T[2] - r[6]*T[0])*pfy + (r[6]*T[1] - r[3]*T[2])*pfx + (r[3]*T[0] - r[0]*T[1])*f) / common_denominator;
}

void tcgOGLTsai3DCameraPosition(double T[3], double r[9], double *pobsx, double *pobsy, double *pobsz)
{
 *pobsx = -(r[0]*T[0] + r[3]*T[1] + r[6]*T[2]);
 *pobsy = -(r[1]*T[0] + r[4]*T[1] + r[7]*T[2]);
 *pobsz = -(r[2]*T[0] + r[5]*T[1] + r[8]*T[2]);
}

void tcgOGLTsai3DOGLModelViewMatrix(double T[3], double r[9], double mv[16])
{
 mv[0] =  r[0]; mv[4] =  r[1]; mv[8]  =  r[2]; mv[12] =  T[0];
 mv[1] =  r[3]; mv[5] =  r[4]; mv[9]  =  r[5]; mv[13] =  T[1];
 mv[2] =  r[6]; mv[6] =  r[7]; mv[10] =  r[8]; mv[14] =  T[2];
 mv[3] = 0.000; mv[7] = 0.000; mv[11] = 0.000; mv[15] = 1.000;
}

void tcgOGLTsai3DOGLProjectionMatrix(double f, double np, double fp, int width, int height, double pr[16])
{
 double b, t, l, r;
 t = np*height/2.0/f;
 b = -t;
 r = np*width/2.0/f;
 l = -r;

 pr[0] = -2.0*np/(r-l); pr[4] =           0.0; pr[8]  =      (r+l)/(r-l); pr[12] =                0.0;
 pr[1] =           0.0; pr[5] = -2.0*np/(t-b); pr[9]  =      (t+b)/(t-b); pr[13] =                0.0;
 pr[2] =           0.0; pr[6] =           0.0; pr[10] = -(fp+np)/(fp-np); pr[14] = -2.0*fp*np/(fp-np);
 pr[3] =           0.0; pr[7] =           0.0; pr[11] =             -1.0; pr[15] =                0.0;
}
