/**
 * \file align_nonlinear.c
 * \brief This file normalizes the camera distortions.
 *
 * We use the Heikkika model described in
 * http://www.vision.caltech.edu/bouguetj/calib_doc/papers/heikkila97.pdf
 *
 * \ingroup corefiles
 * \date May 20, 2009
 * \author Alexander Pimenov
 */
#include <math.h>

#include "g12buffer.h"
#include "align_nonlinear.h"

/**
 * \brief Apply the lens distortion correction.
 *
 * For details see LensCorrectionParametres
 *
 * \param input the buffer to be corrected
 * \param params the intrinsic parameters of the lens
 * \return A corrected buffer
 *
 */
G12Buffer *correctLens (G12Buffer *input, LensCorrectionParametres* params)
{
	/** @todo seem to have major error here. You need a backward transformation
	 * Error is correctied but need addtional tests
	 *
	 */
	int h = input->h;
	int w = input->w;
	int i,j;
	G12Buffer *toReturn = g12BufferCreate(h, w);
	for (i = 0; i < h; i++)
		for (j = 0; j < w; j++)
		{
			// dx and dy will store the coordinate relative to the center point
			int dy = i - params->center.y;
			int dx = j - params->center.x;
			double dxsq = dx * dx;
			double dysq = dy * dy;
			double dxdy = dx * dy;

			double rsq = dxsq + dysq;
			double radialCorrection = params->k1 * rsq +  params->k2 * rsq * rsq;

			double radialX = (double)dx * radialCorrection;
			double radialY = (double)dy * radialCorrection;

			double tangentX = 2 * params->p1 * dxdy + params->p2 * ( rsq + 2 * dxsq );
			double tangentY = params->p1 * (rsq + 2 * dysq) + 2 * params->p2 * dxdy;

			int x = - radialX - tangentX + (double)j;
			int y = - radialY - tangentY + (double)i;


			if (x < 0 || x > input->w - 2  || y < 0 || y > input->h - 2)
			{
				toReturn->data[i * w + j] = 0x80 << 4;
				continue;
			}

			int a = input->data[(int)(y) * input->w  +     (int)(x)];
			int b = input->data[(int)(y) * input->w  +     (int)(x + 1)];
			int c = input->data[(int)(y + 1) * input->w  + (int)(x)];
			int d = input->data[(int)(y + 1) * input->w  + (int)(x + 1)];
			double k1 = x - (int)(x);
			double k2 = y - (int)(y);

			int result = (a * (1 - k1) + k1 * b) * (1 - k2) +
						 (c * (1 - k1) + k1 * d) * k2;
			toReturn->data[i * w + j] = result;
		}

	return toReturn;

}


void CameraModel(double input[], double X, double Y, double Z, double *x, double *y)
{
	// The input layout
	double k1 = input[0];
	double k2 = input[1];

	double p1 = input[2];
	double p2 = input[3];

	double a = input[4];
	double b = input[5];
	double c = input[6];
	double d = input[7];

	double X0 = input[8];
	double Y0 = input[9];
	double Z0 = input[10];

	double f = input[11];

	double sx = input[12];
	double sy = input[13];

	double u0 = input[14];
	double v0 = input[15];

    //
	double len = sqrt (a * a + b * b + c * c + d * d);
	a /= len;
	b /= len;
	c /= len;
	d /= len;

	/**
	 *
	 * [-d^2-c^2+b^2+a^2, 2*b*c-2*a*d,          2*b*d+2*a*c]
	 * [2*b*c+2*a*d,      -d^2+c^2-b^2+a^2,     2*c*d-2*a*b]
	 * [2*b*d-2*a*c,      2*c*d+2*a*b,      d^2-c^2-b^2+a^2])
	 */

	 float da, db, dc, aa, bb, bc, ab, ac, cc, a2, b2, c2;
	 float s  = 2.0f/len;  // 4 mul 3 add 1 div
	        a2 = a * s;    b2 = b * s;    c2 = c * s;
	        aa = a * a2;   ab = a * b2;   ac = a * c2;
	        bb = b * b2;   bc = b * c2;   cc = c * c2;
	        da = d * a2;   db = d * b2;   dc = d * c2;

	 double RX  = (1.0 - (bb + cc)) * X +          (ab - dc) * Y +          (ac + db) * Z;
	 double RY  =       ( ab + dc ) * X + (1.0f - (aa + cc)) * Y +          (bc - da) * Z;
	 double RZ  =         (ac - db) * X +          (bc + da) * Y + (1.0f - (aa + bb)) * Z;


	/*double RX = (-(d*d)-(c*c)+(b*b)+(a*a)) * X +              (2*b*c-2*a*d) * Y +             (2*b*d+2*a*c) * Z;
	double RY =              (2*b*c+2*a*d) * X + (-(d*d)+(c*c)-(b*b)+(a*a)) * Y +             (2*c*d-2*a*b) * Z;
	double RZ =	             (2*b*d-2*a*c) * X +              (2*c*d+2*a*b) * Y + ((d*d)-(c*c)-(b*b)+(a*a)) * Z;
*/
	double MX = RX + X0;
	double MY = RY + Y0;
	double MZ =	RZ + Z0;
	//printf (" Len1: %lf Len2: %lf\n", sqrt(X*X + Y*Y + Z*Z), sqrt(RX*RX + RY*RY + RZ*RZ));

	double PX = (f / MZ) * MX;
	double PY = (f / MZ) * MY;

	double dx = PX * sx;
	double dy = PY * sy;

	double dxsq = dx * dx;
	double dysq = dy * dy;
	double dxdy = dx * dy;

	double rsq = dxsq + dysq;
	double radialCorrection = k1 * rsq +  k2 * rsq * rsq;

	double radialX = (double)dx * radialCorrection;
	double radialY = (double)dy * radialCorrection;

	double tangentX = 2 * p1 * dxdy + p2 * ( rsq + 2 * dxsq );
	double tangentY = p1 * (rsq + 2 * dysq) + 2 * p2 * dxdy;

	*x = (dx + radialX + tangentX) + u0;
	*y = (dy + radialY + tangentY) + v0;

//	*x = dx + u0;
//	*y = dy + v0;


}




void FmodelSimple(double input[], double X, double Y, double Z, double *x, double *y)
{
	double a = input[0];
	double b = input[1];
	double c = input[2];
	double d = input[3];

	double X0 = input[4];
	double Y0 = input[5];
	double Z0 = input[6];

	double f = input[7];

	double sx = input[8];
	double sy = input[9];

	double u0 = input[10];
	double v0 = input[11];

    //
	double len = sqrt (a * a + b * b + c * c + d * d);
	a /= len;
	b /= len;
	c /= len;
	d /= len;

	/**
	 *
	 * [-d^2-c^2+b^2+a^2, 2*b*c-2*a*d,          2*b*d+2*a*c]
	 * [2*b*c+2*a*d,      -d^2+c^2-b^2+a^2,     2*c*d-2*a*b]
	 * [2*b*d-2*a*c,      2*c*d+2*a*b,      d^2-c^2-b^2+a^2])
	 */


	//

	double RX = (-(d*d)-(c*c)+(b*b)+(a*a)) * X +              (2*b*c-2*a*d) * Y +             (2*b*d+2*a*c) * Z;
	double RY =              (2*b*c+2*a*d) * X + (-(d*d)+(c*c)-(b*b)+(a*a)) * Y +             (2*c*d-2*a*b) * Z;
	double RZ =	             (2*b*d-2*a*c) * X +              (2*c*d+2*a*b) * Y + ((d*d)-(c*c)-(b*b)+(a*a)) * Z;

	double MX = RX + X0;
	double MY = RY + Y0;
	double MZ =	RZ + Z0;
	//printf (" Len1: %lf Len2: %lf\n", sqrt(X*X + Y*Y + Z*Z), sqrt(RX*RX + RY*RY + RZ*RZ));

	double PX = (f / MZ) * MX;
	double PY = (f / MZ) * MY;

	double dx = PX * sx;
	double dy = PY * sy;

	*x = dx + u0;
	*y = dy + v0;


}
