/**
 * \file projective.c
 * \brief A module to work with 3D and 2D affine and projective transforms
 *
 *
 * \ingroup corefiles
 * \date   Apr 13, 2009
 * \author Alexander Pimenov
 *
 */

#include "global.h"

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

#include "matrix.h"
#include "projective.h"
/**
 *
 * Creates a 2D projective transform for the rotation on the angle alpha
 *
 * \f[ P=\pmatrix{
 *       cos(\alpha)  & sin(\alpha)  & 0\cr
 *       -sin(\alpha) & cos(\alpha)  & 0\cr
 *                 0  &           0  & 1}
 * \f]
 *
 * \param alpha angle to rotate by
 * \return the rotation matrix
 */
projectiveTransform2D getRotation2D(double alpha)
{
	double c = cos(alpha);
	double s = sin(alpha);
	projectiveTransform2D toReturn;

	toReturn.data[0] = c;
	toReturn.data[1] = s;
	toReturn.data[2] = 0;

	toReturn.data[3] = -s;
	toReturn.data[4] = c;
	toReturn.data[5] = 0;

	toReturn.data[6] = 0;
	toReturn.data[7] = 0;
	toReturn.data[8] = 1;

	return toReturn;
}


/**
 * \brief This function returns a matrix with only the shift components
 *
 * \f[ P=\pmatrix{
 *       1  & 0  & dx\cr
 *       0  & 1  & dy\cr
 *       0  & 0  & 1}
 * \f]
 *
 * \return The shift matrix
 */
projectiveTransform2D getShift2D(double dx, double dy)
{
	projectiveTransform2D toReturn;

	toReturn.data[0] = 1;
	toReturn.data[1] = 0;
	toReturn.data[2] = dx;

	toReturn.data[3] = 0;
	toReturn.data[4] = 1;
	toReturn.data[5] = dy;

	toReturn.data[6] = 0;
	toReturn.data[7] = 0;
	toReturn.data[8] = 1;

	return toReturn;
}

/**
 * \brief This function returns a matrix with only the scale components
 *
 *
 * \f[ P=\pmatrix{
 *       sx & 0  & 0\cr
 *       0  & sy & 0\cr
 *       0  & 0  & 1}
 * \f]
 *
 * \return The scale matrix
 */

projectiveTransform2D getScale2D(double sx, double sy)
{
	projectiveTransform2D toReturn;

	toReturn.data[0] = sx;
	toReturn.data[1] = 0;
	toReturn.data[2] = 0;

	toReturn.data[3] = 0;
	toReturn.data[4] = sy;
	toReturn.data[5] = 0;

	toReturn.data[6] = 0;
	toReturn.data[7] = 0;
	toReturn.data[8] = 1;

	return toReturn;
}

/**
 * \brief This function returns a matrix with only the projective components
 *
 *
 * \f[ P=\pmatrix{
 *        1  & 0  & 0\cr
 *        0  & 1  & 0\cr
 *        px & py & 1}
 * \f]
 *
 */

projectiveTransform2D getProjective2D(double px, double py)
{
	projectiveTransform2D toReturn = {
			{ 1,   0, 0,
			  0,   1, 0,
			  px, py, 1}};
	return toReturn;

}

/**
 * \brief This function returns a matrix filled with the given elements
 *
 *
 * \f[ P=\pmatrix{
 *        a_{1,1} & a_{1,2} & a_{1,3}\cr
 *        a_{2,1} & a_{2,2} & a_{2,3}\cr
 *        a_{3,1} & a_{3,2} & a_{3,3}} \f]
 *
 */
projectiveTransform2D getProjective2DRaw(
		double a11, double a12, double a13,
		double a21, double a22, double a23,
		double a31, double a32, double a33
		)
{
	projectiveTransform2D toReturn;
	toReturn.data[0] = a11; toReturn.data[1] = a12; toReturn.data[2] = a13;
	toReturn.data[3] = a21; toReturn.data[4] = a22; toReturn.data[5] = a23;
	toReturn.data[6] = a31; toReturn.data[7] = a32; toReturn.data[8] = a33;
	return toReturn;
}

affineTransform3D getRotation3DX(double alpha)
{
	double c = cos(alpha);
	double s = sin(alpha);
	affineTransform3D toReturn;
	toReturn.data[0] = 1;	toReturn.data[1] = 0;	toReturn.data[2] = 0;	toReturn.data[3] = 0;

	toReturn.data[4] = 0;	toReturn.data[5] = c;	toReturn.data[6] = s;	toReturn.data[7] = 0;

	toReturn.data[8] = 0;	toReturn.data[9] = -s;	toReturn.data[10] = c;	toReturn.data[11] = 0;

	toReturn.data[12] = 0;	toReturn.data[13] = 0;	toReturn.data[14] = 0;	toReturn.data[15] = 1;
	return toReturn;
}

affineTransform3D getRotation3DY(double alpha)
{
	double c = cos(alpha);
	double s = sin(alpha);
	affineTransform3D toReturn;
	toReturn.data[0] = c;	toReturn.data[1] = 0;	toReturn.data[2] = s;	toReturn.data[3] = 0;

	toReturn.data[4] = 0;	toReturn.data[5] = 1;	toReturn.data[6] = 0;	toReturn.data[7] = 0;

	toReturn.data[8] = -s;	toReturn.data[9] = 0;	toReturn.data[10] = c;	toReturn.data[11] = 0;

	toReturn.data[12] = 0;	toReturn.data[13] = 0;	toReturn.data[14] = 0;	toReturn.data[15] = 1;
	return toReturn;
}

affineTransform3D getRotation3DZ(double alpha)
{
	double c = cos(alpha);
	double s = sin(alpha);
	affineTransform3D toReturn;
	toReturn.data[0] = c;	toReturn.data[1] = s;	toReturn.data[2] = 0;	toReturn.data[3] = 0;

	toReturn.data[4] = -s;	toReturn.data[5] = c;	toReturn.data[6] = 0;	toReturn.data[7] = 0;

	toReturn.data[8] = 0;	toReturn.data[9] = 0;	toReturn.data[10] = 1;	toReturn.data[11] = 0;

	toReturn.data[12] = 0;	toReturn.data[13] = 0;	toReturn.data[14] = 0;	toReturn.data[15] = 1;
	return toReturn;
}

/**
 *  Creates a matrix that rotates around the given axis.
 *
 *
 *  \f[ R = \pmatrix{
 *  	 u_x^2+(1-u_x^2)c  & u_x u_y(1-c)-u_zs & u_x u_z(1-c)+u_ys \cr
 *       u_x u_y(1-c)+u_zs & u_y^2+(1-u_y^2)c  & u_y u_z(1-c)-u_xs \cr
 *       u_x u_z(1-c)-u_ys & u_y u_z(1-c)+u_xs & u_z^2+(1-u_z^2)c }
 *
 *  \f]
 *
 *
 */
affineTransform3D getRotation3DAxis(double alpha, vector3Dd u)
{
	double c = cos(alpha);
	double s = sin(alpha);

	vectorNormalize(&u);

	affineTransform3D toReturn;
	/*toReturn.data[0] = c;	toReturn.data[1] = s;	toReturn.data[2] = 0;	toReturn.data[3] = 0;

	toReturn.data[4] = -s;	toReturn.data[5] = c;	toReturn.data[6] = 0;	toReturn.data[7] = 0;

	toReturn.data[8] = 0;	toReturn.data[9] = 0;	toReturn.data[10] = 1;	toReturn.data[11] = 0;

	toReturn.data[12] = 0;	toReturn.data[13] = 0;	toReturn.data[14] = 0;	toReturn.data[15] = 1;*/
	return toReturn;
}

affineTransform3D getShift3D(double dx, double dy, double dz)
{
	affineTransform3D toReturn;

	toReturn.data[0] = 1;
	toReturn.data[1] = 0;
	toReturn.data[2] = 0;
	toReturn.data[3] = dx;

	toReturn.data[4] = 0;
	toReturn.data[5] = 1;
	toReturn.data[6] = 0;
	toReturn.data[7] = dy;

	toReturn.data[8] = 0;
	toReturn.data[9] = 0;
	toReturn.data[10] = 1;
	toReturn.data[11] = dz;

	toReturn.data[12] = 0;
	toReturn.data[13] = 0;
	toReturn.data[14] = 0;
	toReturn.data[15] = 1;
	return toReturn;
}

affineTransform3D getScale3D(double sx, double sy, double sz)
{
	affineTransform3D toReturn;

	toReturn.data[0] = sx;
	toReturn.data[1] = 0;
	toReturn.data[2] = 0;
	toReturn.data[3] = 0;

	toReturn.data[4] = 0;
	toReturn.data[5] = sy;
	toReturn.data[6] = 0;
	toReturn.data[7] = 0;

	toReturn.data[8] = 0;
	toReturn.data[9] = 0;
	toReturn.data[10] = sz;
	toReturn.data[11] = 0;

	toReturn.data[12] = 0;
	toReturn.data[13] = 0;
	toReturn.data[14] = 0;
	toReturn.data[15] = 1;
	return toReturn;
}

affineTransform3D affine3Dtranspose(affineTransform3D t)
{
	affineTransform3D toReturn;
	toReturn.data[0]  = t.data[0];
	toReturn.data[1]  = t.data[4];
	toReturn.data[2]  = t.data[8];
	toReturn.data[3]  = t.data[12];

	toReturn.data[4]  = t.data[1];
	toReturn.data[5]  = t.data[5];
	toReturn.data[6]  = t.data[9];
	toReturn.data[7]  = t.data[13];

	toReturn.data[8]  = t.data[2];
	toReturn.data[9]  = t.data[6];
	toReturn.data[10] = t.data[10];
	toReturn.data[11] = t.data[14];

	toReturn.data[12] = t.data[3];
	toReturn.data[13] = t.data[7];
	toReturn.data[14] = t.data[11];
	toReturn.data[15] = t.data[15];

	return toReturn;
}

/* Yea! Some black c magic now! */
/**
 * \brief This function returns a product of two matrices
 *
 *
 */
projectiveTransform2D multiplyBy2D(projectiveTransform2D a, projectiveTransform2D b)
{
	projectiveTransform2D toReturn;
	double *z = toReturn.data;
	double *x = a.data;
	double *y = b.data;

	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 3)); (*z) += (*x) * (*(y + 6)); x -= 2; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 3)); (*z) += (*x) * (*(y + 6)); x -= 2; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 3)); (*z) += (*x) * (*(y + 6)); x++   ; y -= 2;	z++;

	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 3)); (*z) += (*x) * (*(y + 6)); x -= 2; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 3)); (*z) += (*x) * (*(y + 6)); x -= 2; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 3)); (*z) += (*x) * (*(y + 6)); x++ ; y -= 2;     z++;

	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 3)); (*z) += (*x) * (*(y + 6)); x -= 2; y++;      z++;
 	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 3)); (*z) += (*x) * (*(y + 6)); x -= 2; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 3)); (*z) += (*x) * (*(y + 6)); x++ ; y -= 2;     z++;


	return toReturn;
}


affineTransform3D multiplyBy3D(affineTransform3D a, affineTransform3D b)
{
	affineTransform3D toReturn;
	double *z = toReturn.data;
	double *x = a.data;
	double *y = b.data;

	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x -= 3; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x -= 3; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x -= 3; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x++   ; y -= 3;   z++;

	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x -= 3; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x -= 3; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x -= 3; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x++   ; y -= 3;	  z++;

	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x -= 3; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x -= 3; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x -= 3; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x++   ; y -= 3;	  z++;

	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x -= 3; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x -= 3; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x -= 3; y++;      z++;
	(*z) = (*x++) * (*y); (*z) += (*x++) * (*(y + 4)); (*z) += (*x++) * (*(y + 8)); (*z) += (*x) * (*(y + 12)); x++   ; y -= 3;	  z++;


	return toReturn;
}

void applyTransform3D (affineTransform3D *transform, double x, double y, double z, double *rx, double *ry, double *rz)
{
	double f =  x * transform->data[12] + y * transform->data[13] + z * transform->data[14] + transform->data[15];
	     *rx = (x * transform->data[0] + y * transform->data[1] + z * transform->data[2] + transform->data[3]) / f;
	     *ry = (x * transform->data[4] + y * transform->data[5] + z * transform->data[6] + transform->data[7]) / f;
	     *rz = (x * transform->data[8] + y * transform->data[9] + z * transform->data[10] + transform->data[11]) / f;
}

void extractQuanternion(affineTransform3D *transform, double *x, double *y, double *z, double *w)
{
	double *data = transform->data;

	double trace = data[0] + data[5] + data[10]; // trace of martix
	double inverse_norm;

	if (trace > 0){
		*x = data[9] - data[6];
		*y = data[2] - data[8];
		*z = data[4] - data[1];
		*w = trace + 1.0;
		inverse_norm = 0.5 / sqrt (*w);
	}
	else
	{
		if( (data[0] > data[5] ) && ( data[0] > data[10]) ) {
			*x = 1 + data[0] - data[5] - data[10];
			*y = data[1] + data[4];
			*z = data[2] + data[8];
			*w = data[9] - data[6];
			inverse_norm = 0.5 / sqrt (*x);
		}else
		if ( data[5] > data[10] ){
			*x = data[1] - data[4];
			*y = 1 + data[5] - data[10] - data[0];
			*z = data[6] + data[9];
			*w = data[2] - data[8];
			inverse_norm = 0.5 / sqrt (*y);
		}else{
			*x = data[2] + data[8];
			*y = data[6] + data[9];
			*z = 1 + data[10] - data[0] - data[5];
			*w = data[4] - data[1];
			inverse_norm = 0.5 / sqrt (*z);
		}
	}

	*x *= inverse_norm;
	*y *= inverse_norm;
	*z *= inverse_norm;
	*w *= inverse_norm;
}


void projective2Dprint(projectiveTransform2D *t)
{
	printf("[ %lg, %lg, %lg],\n", t->data[0], t->data[1], t->data[2]);
	printf("[ %lg, %lg, %lg],\n", t->data[3], t->data[4], t->data[5]);
	printf("[ %lg, %lg, %lg]\n", t->data[6], t->data[7], t->data[8]);
}

projectiveTransform2D projective2Dtranspose(projectiveTransform2D t)
{
	projectiveTransform2D toReturn;
	toReturn.data[0] = t.data[0];
	toReturn.data[1] = t.data[3];
	toReturn.data[2] = t.data[6];

	toReturn.data[3] = t.data[1];
	toReturn.data[4] = t.data[4];
	toReturn.data[5] = t.data[7];

	toReturn.data[6] = t.data[2];
	toReturn.data[7] = t.data[5];
	toReturn.data[8] = t.data[8];
	return toReturn;
}

// TODO: Add math
double projective2Ddeterminant (projectiveTransform2D *t)
{
	return
		+ t->data[0] * (t->data[4] *  t->data[8] - t->data[5] * t->data[7])
		- t->data[1] * (t->data[3] *  t->data[8] - t->data[5] * t->data[6])
		+ t->data[2] * (t->data[3] *  t->data[7] - t->data[6] * t->data[4]);
}

projectiveTransform2D projective2DCreateFromMatrix(Matrix *M)
{
#ifdef ASSERT
	if (M->m != 3 || M->n != 3)
		DOASSERT("Wrong Matrix Size\n");
#endif
	// TODO: redo this.
	projectiveTransform2D toReturn;
	int i;
	for (i = 0; i < 9; i++)
	{
		toReturn.data[i] = M->data[i];
	}
	//memcpy(&toReturn.data, M->data, sizeof (double) * 9);
	return toReturn;
}

/**
 * \brief This function returns an inverse of the matrix.
 *
 * The calculation is done straight forward using the following formular property
 *
 *  \f[ I^{-1} = \frac 1 {|I|} \pmatrix{ M_{1,1} && \cdots }
 * \f]
 *
 *
 * \attention No check is done, if the matrix is invertable or not
 *
 *
 *
 * \return The \f$I^{-1}\f$
 */
projectiveTransform2D projective2Dinverse(projectiveTransform2D *t)
{
	projectiveTransform2D toReturn;
	double det = projective2Ddeterminant(t);
#ifdef ASSERTS
	if (det == 0)
		DOASSERT("Matrix is not invertable");
#endif

	toReturn.data[0] = + (t->data[4] * t->data[8] - t->data[5] * t->data[7]) / det;
	toReturn.data[3] = - (t->data[3] * t->data[8] - t->data[5] * t->data[6]) / det;
	toReturn.data[6] = + (t->data[3] * t->data[7] - t->data[4] * t->data[6]) / det;

	toReturn.data[1] = - (t->data[1] * t->data[8] - t->data[2] * t->data[7]) / det;
	toReturn.data[4] = + (t->data[0] * t->data[8] - t->data[2] * t->data[6]) / det;
	toReturn.data[7] = - (t->data[0] * t->data[7] - t->data[1] * t->data[6]) / det;

	toReturn.data[2] = + (t->data[1] * t->data[5] - t->data[2] * t->data[4]) / det;
	toReturn.data[5] = - (t->data[0] * t->data[5] - t->data[2] * t->data[3]) / det;
	toReturn.data[8] = + (t->data[0] * t->data[4] - t->data[3] * t->data[1]) / det;
	return toReturn;
}

projectiveTransform2D projective2DCreateFromRaw (double *data)
{
	projectiveTransform2D toReturn;
	memcpy(&toReturn.data, data, sizeof(double) * 9);
	return toReturn;
}

/**
 *
 */
vector2Dd projective2DapplyToCartesian(projectiveTransform2D *t, vector2Dd input)
{
	vector2Dd toReturn;
	double z = t->data[6] * input.x + t->data[7] * input.y + t->data[8];
	toReturn.x =  t->data[0] * input.x + t->data[1] * input.y + t->data[2];
	toReturn.y =  t->data[3] * input.x + t->data[4] * input.y + t->data[5];

	toReturn.x /= z;
	toReturn.y /= z;
	return toReturn;
}


vector3Dd projective2Dapply(projectiveTransform2D *t, vector3Dd *input)
{
	vector3Dd toReturn;
	toReturn.x = t->data[0] * input->x + t->data[1] * input->y + t->data[2] * input->z;
	toReturn.y = t->data[3] * input->x + t->data[4] * input->y + t->data[5] * input->z;
	toReturn.z = t->data[6] * input->x + t->data[7] * input->y + t->data[8] * input->z;
	return toReturn;
}
