/*============================================================================
	Title: SloanSHRotate.cpp
	Module: Pi/MathLib
	Author: Ignacio Castaņo
	Date: 03/10/2004
============================================================================*/

/*----------------------------------------------------------------------------
	Doc:
----------------------------------------------------------------------------*/

/** @file SloanSHRotate.cpp
 * @brief Spherical Harmonic rotation using Peter-Pike Sloan rotation method.
**/


/*----------------------------------------------------------------------------
	Includes:
----------------------------------------------------------------------------*/

// MathLib
#include "SloanSHRotate.h"
#include "SphericalHarmonic.h"


/*----------------------------------------------------------------------------
	Private:
----------------------------------------------------------------------------*/

namespace {

	template <int B>
	struct MatrixBand {
	
		enum {
			ROWS = 2 * B + 1,
			SIZE = ROWS * ROWS
		};
	
		/** Get sh band. */
		void GetBand( const ShMatrix * shrm ) {
			for(int y = -B; y <= B; y++) {
				for(int x = -B; x <= B; x++) {
					GetElem(x+B, y+B) = shrm->GetElem(B, x, y);
				}
			}
		}

		/** Set sh band. */
		void SetBand( ShMatrix * shrm ) {
			for(int y = -B; y <= B; y++) {
				for(int x = -B; x <= B; x++) {
					shrm->GetElem(B, x, y) = GetElem(x+B, y+B);
				}
			}
		}
		
		/** Get an element of this band. */
		float GetElem(int x, int y) const {
			return e[x + ROWS * y];
		}
		
		/** Get an element of this band. */
		float & GetElem(int x, int y) {
			return e[x + ROWS * y];
		}
		
		/** Clear this band. */
		void Clear() {
			memset(e, 0, sizeof(float)*SIZE);
		}

		/** Multiply this band. */
		void Multiply(MatrixBand & A) {
			float v[ROWS];
			
			for(int i = 0; i < ROWS; i++) {
				for(int j = 0; j < ROWS; j++) {
					v[j] = GetElem(i, j);
				}
			
				for(int j = 0; j < ROWS; j++) {
					float elem = 0;
					
					for(int k = 0; k < ROWS; k++) {
						elem += v[k] * A.GetElem(k, j);
					}
					
					GetElem(i, j) = elem;
				}
			}
		}

	
		void RotationZ(float alpha) {
			Clear();

			GetElem(B+0, B+0) = 1.0f;
			
			for( int m = 1; m <= B; m++ ) {
				GetElem(B-m, B-m) = cos(m * alpha);
				GetElem(B-m, B+m) =-sin(m * alpha);
				GetElem(B+m, B-m) = sin(m * alpha);
				GetElem(B+m, B+m) = cos(m * alpha);
			}
		}
		
		void Print() {
			for( int ma = -B; ma <= B; ma++ ) {
				piDebug("[ ");
				for( int mb = -B; mb <= B; mb++ ) {
					piDebug("%5.2f ", GetElem(B+ma, B+mb));
				}
				piDebug("]\n");
			}
		}		
		
		// Elements of this band.
		float e[SIZE];
	};
	

	struct MatrixBand2 : public MatrixBand<2> {

		void SetColumn(int y, float a, float b, float c, float d, float f) {
			e[y*5+0] = a; e[y*5+1] = b; e[y*5+2] = c; e[y*5+3] = d; e[y*5+4] = f;
		}
	
		void RotationY(float alpha) {
			float ca = cos(alpha);
			float sa = sin(alpha);
			float c2a = cos(2*alpha);
			float s2a = sin(2*alpha);
			
			SetColumn(0, ca, sa, 0, 0, 0);
			SetColumn(1, -sa, ca, 0, 0, 0);
			SetColumn(2, 0, 0, 3.0f/4.0f * c2a + 1.0f/4.0f, -sqrt(3.0f)/2.0f * s2a, -sqrt(3.0f)/4.0f * c2a + sqrt(3.0f)/4.0f);
			SetColumn(3, 0, 0, sqrt(3.0f)/2.0f * s2a, c2a, -1.0f/2.0f * s2a);
			SetColumn(4, 0, 0, -sqrt(3.0f)/4.0f * c2a + sqrt(3.0f)/4.0f, 1.0f/2.0f * s2a, 3.0f/4.0f + c2a/4.0f);
		}

		void RotationZYZ(float s, float t, float r) {
		
			/* GiNaC code:
			matrix Rs(5, 5), Rt(5, 5), Rr(5, 5);
			
			Rt = ct, st, 0, 0, 0,
				-st, ct, 0, 0, 0,
				0, 0, a, -b, c
				0, 0, b, c2a, -d,
				0, 0, c, d, e;
				
			Rs = c2s, 0, 0, 0, s2s,
				0, cs, 0, ss, 0,
				0, 0, 1, 0, 0,
				0, -ss, 0, cs, 0,
				-s2s, 0, 0, 0, c2s;

			Rr = c2r, 0, 0, 0, s2r,
				0, cr, 0, sr, 0,
				0, 0, 1, 0, 0,
				0, -sr, 0, cr, 0,
				-s2r, 0, 0, 0, c2r;
			ex R = (Rs * Rt) * Rr;
			*/

			// GiNaC output:
			//[c2s*ct*c2r-s2s*s2r*e, c2s*st*cr-d*s2s*sr, s2s*c, d*s2s*cr+c2s*sr*st, c2s*ct*s2r+s2s*c2r*e],
			//[d*s2r*ss-cs*st*c2r, cs*cr*ct-c2t*sr*ss, b*ss,cs*sr*ct+c2t*cr*ss, -d*ss*c2r-cs*st*s2r],
			//[-c*s2r, b*sr, a, -b*cr, c*c2r],
			//[cs*d*s2r+st*ss*c2r, -cs*c2t*sr-cr*ct*ss, cs*b, -sr*ct*ss+cs*c2t*cr, -cs*d*c2r+st*s2r*ss],
			//[-s2s*ct*c2r-c2s*s2r*e, -d*c2s*sr-s2s*st*cr, c2s*c, -s2s*sr*st+d*c2s*cr, c2s*c2r*e-s2s*ct*s2r]

			float cs = cos(s);
			float ss = sin(s);
			float c2s = cos(2*s);
			float s2s = sin(2*s);
			
			float ct = cos(t);
			float st = sin(t);
			float c2t = cos(2*t);
			float s2t = sin(2*t);

			float cr = cos(r);
			float sr = sin(r);
			float c2r = cos(2*r);
			float s2r = sin(2*r);
						
			float a = 3.0f / 4.0f * c2t + 1.0f / 4.0f;
			float b = sqrt(3.0f) / 2.0f * s2t;
			float c = -sqrt(3.0f) / 4.0f * c2t + sqrt(3.0f) / 4.0f;
			float d = 1.0f / 2.0f * s2t;
			float e = 3.0f / 4.0f + c2t / 4.0f;

			SetColumn(0, c2s*ct*c2r-s2s*s2r*e, c2s*st*cr-d*s2s*sr, s2s*c, d*s2s*cr+c2s*sr*st, c2s*ct*s2r+s2s*c2r*e);
			SetColumn(1, d*s2r*ss-cs*st*c2r, cs*cr*ct-c2t*sr*ss, b*ss,cs*sr*ct+c2t*cr*ss, -d*ss*c2r-cs*st*s2r);
			SetColumn(2, -c*s2r, b*sr, a, -b*cr, c*c2r);
			SetColumn(3, cs*d*s2r+st*ss*c2r, -cs*c2t*sr-cr*ct*ss, cs*b, -sr*ct*ss+cs*c2t*cr, -cs*d*c2r+st*s2r*ss);
			SetColumn(4, -s2s*ct*c2r-c2s*s2r*e, -d*c2s*sr-s2s*st*cr, c2s*c, -s2s*sr*st+d*c2s*cr, c2s*c2r*e-s2s*ct*s2r);
		}
	};


} // namespace


/*----------------------------------------------------------------------------
	Functions:
----------------------------------------------------------------------------*/


void SloanSHRotate(const Matrix & R, ShMatrix * shrm) {
	piCheck(shrm->GetBandNum() == 3);

	// Band zero.
	shrm->GetElem(0, 0, 0) = 1.0f;
	
	// The first band is rotated by a permutation of the original matrix.
	shrm->GetElem(1, -1, -1) = R(1, 1);
	shrm->GetElem(1, -1,  0) = R(1, 2);
	shrm->GetElem(1, -1, +1) = R(1, 0);
	shrm->GetElem(1,  0, -1) = R(2, 1);
	shrm->GetElem(1,  0,  0) = R(2, 2);
	shrm->GetElem(1,  0, +1) = R(2, 0);
	shrm->GetElem(1, +1, -1) = R(0, 1);
	shrm->GetElem(1, +1,  0) = R(0, 2);
	shrm->GetElem(1, +1, +1) = R(0, 0);

	// Compute ZYZ Euler angle decomposition.
	float s, t, r;
	R.GetEulerAnglesZYZ(s, t, r);

	/*MatrixBand2 Rs, Rt, Rr;
	
	Rs.RotationZ(s);
	Rt.RotationY(t);
	Rr.RotationZ(r);

	MatrixBand2 & B = Rs;
	B.Multiply(Rt);
	B.Multiply(Rr);
	B.SetBand(shrm);*/
	
	MatrixBand2 B;
	B.RotationZYZ(s, t, r);
	B.SetBand(shrm);
}