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

void lorentz_params_from_matrix(double *m, double * p) {
	/* Break out the parameters for boost. */
	double sinh_vz = -LMREF(m,3,0);
	double vz = asinh(sinh_vz);
	double cosh_vz = cosh(vz);
	double sinh_vy = -LMREF(m,2,0)/cosh_vz;
	double vy = asinh(sinh_vy);
	double cosh_vy = cosh(vy);
	double sinh_vx = -LMREF(m,1,0)/(cosh_vy*cosh_vz);
	double vx = asinh(sinh_vx);
	double cosh_vx = cosh(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];
	
	memset(Binv,0,16*sizeof(double));
	memcpy(R,m,16*sizeof(double));
	
	/* Here we remove the last Bx */
	LMSET(Binv,0,0,cosh_vx);
	LMSET(Binv,0,1,sinh_vx);
	LMSET(Binv,1,0,sinh_vx);
	LMSET(Binv,1,1,cosh_vx);
	LMSET(Binv,2,2,1.0);
	LMSET(Binv,3,3,1.0);
	
	cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,4,4,4,1.0,Binv,4,R,4,0.0,R,4);
	
	/* Reset Binv, and remove By */
	memset(Binv,0,16*sizeof(double));
	
	LMSET(Binv,0,0,cosh_vy);
	LMSET(Binv,0,2,sinh_vy);
	LMSET(Binv,1,1,1.0);
	LMSET(Binv,2,0,sinh_vy);
	LMSET(Binv,2,2,cosh_vy);
	LMSET(Binv,3,3,1.0);
	
	cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,4,4,4,1.0,Binv,4,R,4,0.0,R,4);
	
	/* Reset Binv and remove */
	memset(Binv,0,16*sizeof(double));
	
	LMSET(Binv,0,0,cosh_vz);
	LMSET(Binv,0,3,sinh_vz);
	LMSET(Binv,1,1,1.0);
	LMSET(Binv,2,2,1.0);
	LMSET(Binv,3,0,sinh_vz);
	LMSET(Binv,3,3,cosh_vz);
	
	cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,4,4,4,1.0,Binv,4,R,4,0.0,R,4);
	
	/* 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 = cosh(v);
	double 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,1,1.0);
	LMSET(m,2,2,1.0);
	LMSET(m,3,0,-sinh_v);
	LMSET(m,3,3,cosh_v);
}

void lorentz_By(double v, double * m) {
	double cosh_v = cosh(v);
	double 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 = cosh(v);
	double 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 Mtemp[16];
	
	/* Clear matrices. */
	memset(Mtemp,0,16*sizeof(double));
	memset(m,0,16*sizeof(double));
	
	/* m <- I */
	LMSET(m, 0, 0, 1.0);
	LMSET(m, 1, 1, 1.0);
	LMSET(m, 2, 2, 1.0);
	LMSET(m, 3, 3, 1.0);
	
	/* Now accumulate the product, starting with Rz (on the right). */
	lorentz_Rz(params[5], Mtemp);
	cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,4,4,4,1.0,Mtemp,4,m,4,0.0,m,4);
	
	/* Now get Ry */
	lorentz_Ry(params[4], Mtemp);	
	cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,4,4,4,1.0,Mtemp,4,m,4,0.0,m,4);
	
	/* Now get Rx */
	lorentz_Rx(params[3], Mtemp);
	cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,4,4,4,1.0,Mtemp,4,m,4,0.0,m,4);
	
	/* m Has now accumulated all the rotations.  Now we need to put in the boosts.  Starting with Bz. */
	lorentz_Bz(params[2], Mtemp);	
	cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,4,4,4,1.0,Mtemp,4,m,4,0.0,m,4);	
	
	/* Now get By. */
	lorentz_By(params[1], Mtemp);
	cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,4,4,4,1.0,Mtemp,4,m,4,0.0,m,4);	

	/* Now get Bz. */
	lorentz_Bz(params[0], Mtemp);
	cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,4,4,4,1.0,Mtemp,4,m,4,0.0,m,4);	
	
	/* 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[] = {
		Bx, By, Bz, Rx, Ry, Rz
	};
	double *Ts[] = {
		Tvx, Tvy, Tvz, Tx, Ty, 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];
		
		/* 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 = 5; j >= 0; j--) {
			if (i == j) {
				/* Accumulate generator onto product */
				cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,4,4,4,1.0,Ts[i],4,dM,4,0.0,dM,4);
			}
			
			cblas_dgemm(CblasColMajor,CblasNoTrans,CblasNoTrans,4,4,4,1.0,Ms[i],4,dM,4,0.0,dM,4);
		}
	}
	
	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;
}