/* SO31.c : Implementation of canonical representation of Lorentz group.
Copyright (C) 2007 Will M. Farr <farr@mit.edu>

This program is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License along
with this program; if not, write to the Free Software Foundation, Inc.,
51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/

#include <SO31.h>

static void mmul_into(double *m1, double *m2, double *m3) {
	size_t i, j, k;
	
	for (i = 0; i < 4; i++) {
		for (j = 0; j < 4; j++) {
			double sum = 0.0;
			for (k = 0; k < 4; k++) {
				sum += LMREF(m1, i, k)*LMREF(m2, k, j);
			}
			LMSET(m3,i,j,sum);
		}
	}
}

void lorentz_params_from_matrix(double *m, double * p) {
	/* Break out the parameters for boost. */
	double sinh_vz, vz, cosh_vz, sinh_vy, vy, cosh_vy, sinh_vx, vx;

	/* These will be useful later */
	double x, sin_x;
	double y, cos_y, sin_y;
	double z, cos_z, sin_z;
	
	/* Now mulitply by inverse boost to get rotation matrix. */
	double Binv[16], R[16], R2[16];
	
	
	sinh_vz = -LMREF(m,3,0);
	vz = asinh(sinh_vz);
	cosh_vz = cosh(vz);
	sinh_vy = -LMREF(m,2,0)/cosh_vz;
	vy = asinh(sinh_vy);
	cosh_vy = cosh(vy);
	sinh_vx = -LMREF(m,1,0)/(cosh_vy*cosh_vz);
	vx = asinh(sinh_vx);
	
	memcpy(R,m,16*sizeof(double));
	
	lorentz_Bx(-vx, Binv);	
	mmul_into(Binv, m, R);
	
	lorentz_By(-vy, Binv);
	mmul_into(Binv, R, R2);
	
	lorentz_Bz(-vz, Binv);
	mmul_into(Binv, R2, R);
	
	
	/* Now R is pure rotation matrix. */
	
	sin_y = - LMREF(R,1,3);
	y = asin(sin_y);
	cos_y = cos(y);
	
	sin_z = -LMREF(R,1,2)/cos_y;
	z = asin(sin_z);
	cos_z = cos(z);
	
	sin_x = LMREF(R,3,2)*cos_z + LMREF(R,3,1)*sin_z;
	x = asin(sin_x);
	
	p[0] = vx;
	p[1] = vy;
	p[2] = vz;
	p[3] = x;
	p[4] = y;
	p[5] = z;
	
	return;
}

void lorentz_Rx(double x, double *m) {
	double cos_x = cos(x);
	double sin_x = sin(x);
	
	memset(m,0,16*sizeof(double));
	LMSET(m,0,0,1.0);
	LMSET(m,1,1,1.0);
	LMSET(m,2,2,cos_x);
	LMSET(m,2,3,-sin_x);
	LMSET(m,3,2,sin_x);
	LMSET(m,3,3,cos_x);
	
	return;
}

void lorentz_Ry(double x, double *m) {
	double cos_x = cos(x);
	double sin_x = sin(x);
	
	memset(m,0,16*sizeof(double));
	LMSET(m,0,0,1.0);
	LMSET(m,1,1,cos_x);
	LMSET(m,1,3,-sin_x);
	LMSET(m,2,2,1.0);
	LMSET(m,3,1,sin_x);
	LMSET(m,3,3,cos_x);
}

void lorentz_Rz(double x, double *m) {
	double cos_x = cos(x);
	double sin_x = sin(x);
	
	memset(m, 0, 16*sizeof(double));
	
	LMSET(m,0,0,1.0);
	LMSET(m,1,1,cos_x);
	LMSET(m,1,2,-sin_x);
	LMSET(m,2,1,sin_x);
	LMSET(m,2,2,cos_x);
	LMSET(m,3,3,1.0);
}

void lorentz_Bx(double v, double *m) {
	double cosh_v, sinh_v;
	
	cosh_v = cosh(v);
	sinh_v = sinh(v);
	
	memset(m,0,16*sizeof(double));
	LMSET(m,0,0,cosh_v);
	LMSET(m,0,1,-sinh_v);
	LMSET(m,1,0,-sinh_v);
	LMSET(m,1,1,cosh_v);
	LMSET(m,2,2,1.0);
	LMSET(m,3,3,1.0);
}

void lorentz_By(double v, double * m) {
	double cosh_v, sinh_v;
	
	cosh_v = cosh(v);
	sinh_v = sinh(v);
	
	memset(m,0,16*sizeof(double));
	LMSET(m,0,0,cosh_v);
	LMSET(m,0,2,-sinh_v);
	LMSET(m,1,1,1.0);
	LMSET(m,2,0,-sinh_v);
	LMSET(m,2,2,cosh_v);
	LMSET(m,3,3,1.0);
}

void lorentz_Bz(double v, double * m) {
	double cosh_v, sinh_v;
	
	cosh_v = cosh(v);
	sinh_v = sinh(v);
	
	memset(m,0,16*sizeof(double));
	LMSET(m,0,0,cosh_v);
	LMSET(m,0,3,-sinh_v);
	LMSET(m,1,1,1.0);
	LMSET(m,2,2,1.0);
	LMSET(m,3,0,-sinh_v);
	LMSET(m,3,3,cosh_v);
}

void lorentz_matrix_from_params(double * params,double * m) {
	double B_or_R[16], Mtemp1[16], Mtemp2[16];
	
	/* Now accumulate the product, starting with Rz (on the right). */
	lorentz_Rz(params[5], Mtemp1);

	/* Now get Ry */
	lorentz_Ry(params[4], B_or_R);	
	mmul_into(B_or_R, Mtemp1, Mtemp2);

	/* Now get Rx */
	lorentz_Rx(params[3], B_or_R);
	mmul_into(B_or_R, Mtemp2, Mtemp1);
	
	/* m Has now accumulated all the rotations.  Now we need to put in the boosts.  Starting with Bz. */
	lorentz_Bz(params[2], B_or_R);
	mmul_into(B_or_R, Mtemp1, Mtemp2);
		
	/* Now get By. */
	lorentz_By(params[1], B_or_R);
	mmul_into(B_or_R, Mtemp2, Mtemp1);

	/* Now get Bx. */
	lorentz_Bx(params[0], B_or_R);
	mmul_into(B_or_R, Mtemp1, m);
	
	/* Now m should contain the LT. */

	return;
}

static double Tx[] = {
	0.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 0.0, 1.0,
	0.0, 0.0, -1.0, 0.0
};

static double Ty[] = {
	0.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 0.0, 1.0,
	0.0, 0.0, 0.0, 0.0,
	0.0, -1.0, 0.0, 0.0
};

static double Tz[] = {
	0.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 1.0, 0.0,
	0.0, -1.0, 0.0, 0.0,
	0.0, 0.0, 0.0, 0.0
};

static double Tvx[] = {
	0.0, -1.0, 0.0, 0.0,
	-1.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 0.0, 0.0
};

static double Tvy[] = {
	0.0, 0.0, -1.0, 0.0,
	0.0, 0.0, 0.0, 0.0,
	-1.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 0.0, 0.0
};

static double Tvz[] = {
	0.0, 0.0, 0.0, -1.0,
	0.0, 0.0, 0.0, 0.0,
	0.0, 0.0, 0.0, 0.0,
	-1.0, 0.0, 0.0, 0.0
};

void lorentz_deriv_matrix_at_params(double *p, double **dMs) {
	int i;
	double Rx[16], Ry[16], Rz[16], Bx[16], By[16], Bz[16];
	double *Ms[6];
	double *Ts[6];
	
	Ms[0] = Bx;
	Ms[1] = By;
	Ms[2] = Bz;
	Ms[3] = Rx;
	Ms[4] = Ry;
	Ms[5] = Rz;
	
	Ts[0] = Tvx;
	Ts[1] = Tvy;
	Ts[2] = Tvz;
	Ts[3] = Tx;
	Ts[4] = Ty;
	Ts[5] = Tz;
	
	/* Fill Rs and Bs with values for the given LT. */
	lorentz_Rx(p[3], Rx);
	lorentz_Ry(p[4], Ry);
	lorentz_Rz(p[5], Rz);
	lorentz_Bx(p[0], Bx);
	lorentz_By(p[1], By);
	lorentz_Bz(p[2], Bz);
	
	for (i = 0; i < 6; i++) {
		int j;
		double *dM = dMs[i];
		double Mtemp[16];
		
		/* Clear dM */
		memset(dM, 0, 16*sizeof(double));
		LMSET(dM, 0, 0, 1.0);
		LMSET(dM, 1, 1, 1.0);
		LMSET(dM, 2, 2, 1.0);
		LMSET(dM, 3, 3, 1.0);
		
		for (j = 0; j < 6; j++) {
			mmul_into(dM, Ms[j], Mtemp);
			memcpy(dM, Mtemp, 16*sizeof(double));
			
			if (i == j) {
				mmul_into(dM, Ts[j], Mtemp);
				memcpy(dM, Mtemp, 16*sizeof(double));
			}
		}
	}
	
	return;
}

void lorentz_deriv_matrix_at_matrix(double * m, double * * dM) {
	double p[6];
	
	lorentz_params_from_matrix(m, p);
	
	lorentz_deriv_matrix_at_params(p, dM);
	
	return;
}

void params_to_inverse_params(double *p, double *pinv) {
	double Minv[16], Mtemp1[16], Mtemp2[16], L[16];
	
	lorentz_Bx(-p[0],Mtemp1);

	lorentz_By(-p[1],L);
	mmul_into(L, Mtemp1, Mtemp2);
	
	lorentz_Bz(-p[2],L);
	mmul_into(L, Mtemp2, Mtemp1);
	
	lorentz_Rx(-p[3],L);
	mmul_into(L, Mtemp1, Mtemp2);
	
	lorentz_Ry(-p[4],L);
	mmul_into(L, Mtemp2, Mtemp1);
	
	lorentz_Rz(-p[5],L);
	mmul_into(L, Mtemp1, Minv);
	
	lorentz_params_from_matrix(Minv, pinv);
}

void params_compose(double *p1, double *p2, double *pc) {
	double M1[16], M2[16], Mc[16];
	
	lorentz_matrix_from_params(p1, M1);
	lorentz_matrix_from_params(p2, M2);
	
	mmul_into(M1, M2, Mc);
	
	lorentz_params_from_matrix(Mc,pc);
}