#include <iostream>	// for form() - no longer
#include <math.h>	// for sqrt()
#include "Pointlist.h"
#include "Quaternion.h"

// define DEBUG_EIGEN if you want debugging trace
// #define DEBUG_EIGEN 1


static void Compute_SOP(double Array[4][4], const Pointlist &A, 
	const Pointlist &B, 
	const float * weight)
{
    register double 
	    Sxx=0, Sxy=0, Sxz=0, Syx=0, Syy=0, Syz=0, Szx=0, Szy=0, Szz=0;
	    // entries of the 4x4 symmetric N-matrix	
    assert(A.num_points() == B.num_points());
    int num = A.num_points();
    assert (num>0);

    if (!weight)
    {
	for(int i=0;i<num;i++)
	{
	    const XYZpoint& ap=A[i];
	    assert(ap.is_finite());
	    const XYZpoint& bp=B[i];
	    assert(bp.is_finite());

	    Sxx += ap.X * bp.X;
	    Sxy += ap.X * bp.Y;
	    Sxz += ap.X * bp.Z;

	    Syx += ap.Y * bp.X;
	    Syy += ap.Y * bp.Y;
	    Syz += ap.Y * bp.Z;

	    Szx += ap.Z * bp.X;
	    Szy += ap.Z * bp.Y;
	    Szz += ap.Z * bp.Z;
	}
    }
    else
    {
	for(int i=0;i<num;i++)
	{
	    double wt = weight[i];
	    if (wt==0) continue;
//		cerr << "wt=" << wt << "\n";
	    const XYZpoint& ap=A[i];
	    assert(ap.is_finite());
	    const XYZpoint& bp=B[i];
	    assert(bp.is_finite());

	    double bpx = bp.X * wt;
	    double bpy = bp.Y * wt;
	    double bpz = bp.Z * wt;

	    Sxx += ap.X * bpx;
	    Sxy += ap.X * bpy;
	    Sxz += ap.X * bpz;

	    Syx += ap.Y * bpx;
	    Syy += ap.Y * bpy;
	    Syz += ap.Y * bpz;

	    Szx += ap.Z * bpx;
	    Szy += ap.Z * bpy;
	    Szz += ap.Z * bpz;
	}
    }

    Array[0][0] = Sxx + Syy + Szz;
    Array[1][0] = Array[0][1] = Syz - Szy;
    Array[2][0] = Array[0][2] = Szx - Sxz;
    Array[3][0] = Array[0][3] = Sxy - Syx;
    Array[1][1] = Sxx - Syy - Szz;
    Array[2][1] = Array[1][2] = Sxy + Syx;
    Array[3][1] = Array[1][3] = Szx + Sxz;
    Array[2][2] = -Sxx + Syy - Szz;
    Array[3][2] = Array[2][3] = Syz + Szy;
    Array[3][3] = -Sxx - Syy + Szz; 			
}

// Normalize a vector to be of unit length,
// with the first coordinate non-negative.
// Note: the restriction to non-negative first coordinates is
// arbitrary and probably not necessary to the rest of the program.
inline void normalize(double vect[4])
{   double scale = 1./sqrt(vect[0]*vect[0]
    			+ vect[1]*vect[1]
    			+ vect[2]*vect[2]
    			+ vect[3]*vect[3]
			   );
    vect[0] *= scale;
    if (vect[0] < 0) 
    {   vect[0] = 0-vect[0];
        scale = 0-scale;
    }
    vect[1] *= scale;
    vect[2] *= scale;
    vect[3] *= scale;
}

// The following routines are based on the methods in
// Numerical Recipes in C.
// I've been a little sloppier about numerical accuracy of eigenvalues
// (since I only need to figure out which is biggest),
// and I've cut out some of the optimizations for skipping rotations.

static inline void OneRot(double mat[4][4], int i, int j, int k, int l,
	double s, double tau)
{   
    // save the current values for computation
    double mat_ij = mat[i][j];
    double mat_kl = mat[k][l];
    
    mat[i][j] -= s*(mat_kl + tau*mat_ij);
    mat[k][l] += s*(mat_ij - tau*mat_kl);
}

static void JacobiRot(double mat[4][4], double eigen[4][4], 
	int p, int q)
{
    assert (p<q);
    
    // get element to zero
    double mat_pq = mat[p][q];
    if (mat_pq==0.) return;
    
    
    double diff = (mat[q][q]-mat[p][p]);
    
    // get tangent of angle
    double t;
    if (static_cast<float>(diff + 100*mat_pq) == static_cast<float>(diff))
    {    t = mat_pq/diff;
    }
    else
    {   double cotangent = diff/(2*mat_pq);
    	t = cotangent>0?
		 1/(cotangent + sqrt(1+cotangent*cotangent))
		: 1/(cotangent - sqrt(1+cotangent*cotangent));
    }
    
    // get sine and cosine of rotation from tangent
    double c = 1.0/sqrt(1. + t*t);
    double s = c*t;
    
    // compute tangent of half-angle
    double tau = s/(1.0+c);
    
    // compute diagonal correction
    double qplus = t*mat[p][q];
    
    // update matrix[i][j] for i<=j
    mat[p][q] = 0;
    mat[p][p] -= qplus;
    mat[q][q] += qplus;
    
    for (int i=0; i<p; i++)
    {	OneRot(mat, i,p, i,q, s,tau);
    }
    for (int i=p+1; i<q; i++)
    {	OneRot(mat, p,i, i,q, s,tau);
    }
    for (int i=q+1; i<4; i++)
    {	OneRot(mat, p,i, q,i, s,tau);
    }
    
    // update the eigenvector matrix
    for (int i=0; i<4; i++)
    {    OneRot(eigen, i,p, i,q, s,tau);
    }    
}

// Given a 4x4 matrix, try to find the eigenvector for the largest eigenvalue.
static void eigen4(double orig_mat[4][4], double vect[4])
{
    #define MAX_ITER  6 // The maximum number of iterations permitted.

    double vectors[4][4]={ {1,0,0,0},  
    			   {0,1,0,0},
			   {0,0,1,0},
			   {0,0,0,1}};	// collect eigenvectors here.

     // copy the matrix
     double mat[4][4];
     mat[0][0] = orig_mat[0][0];
     mat[0][1] = orig_mat[0][1];
     mat[0][2] = orig_mat[0][2];
     mat[0][3] = orig_mat[0][3];

     mat[1][0] = orig_mat[1][0];
     mat[1][1] = orig_mat[1][1];
     mat[1][2] = orig_mat[1][2];
     mat[1][3] = orig_mat[1][3];

     mat[2][0] = orig_mat[2][0];
     mat[2][1] = orig_mat[2][1];
     mat[2][2] = orig_mat[2][2];
     mat[2][3] = orig_mat[2][3];

     mat[3][0] = orig_mat[3][0];
     mat[3][1] = orig_mat[3][1];
     mat[3][2] = orig_mat[3][2];
     mat[3][3] = orig_mat[3][3];
     
     #ifdef DEBUG_EIGEN
     cerr << "iter eigenvalue\t: eigenvector\n";
     #endif
    
    // Start iteration for eigenvalue and eigenvector.
    int iter;
    for (iter = 0; iter < MAX_ITER; iter++)
    {
	JacobiRot(mat,vectors, 0,1);
	JacobiRot(mat,vectors, 0,2);
	JacobiRot(mat,vectors, 0,3);
	JacobiRot(mat,vectors, 1,2);
	JacobiRot(mat,vectors, 1,3);
	JacobiRot(mat,vectors, 2,3);
    	
	double sum = fabs(mat[0][1]) + fabs(mat[0][2]) + fabs(mat[0][3]) +
		fabs(mat[1][2]) + fabs(mat[1][3]) + fabs(mat[2][3]);
	if (sum< 1.e-10) break;
    }

    if (iter >=MAX_ITER)
    {    cerr << "Warning: eigen4 may not have converged\n";
    }
    
    // select vector for most positive eigenvalue
    int maxat=0;
    double biggest=mat[0][0];
    for (int i=1; i<4; i++)
    {   double diag = mat[i][i];
    	if (diag > biggest)
	{    biggest=diag;
	    maxat = i;
	}
    }

    vect[0] = vectors[0][maxat];
    vect[1] = vectors[1][maxat];
    vect[2] = vectors[2][maxat];
    vect[3] = vectors[3][maxat];
    normalize(vect);
#ifdef DEBUG_EIGEN
    cerr << form("%3d %9g\t:  %7.4f %7.4f %7.4f %7.4f\n", iter, 
	    mat[maxat][maxat],
	    vect[0], vect[1], vect[2], vect[3])
	    << flush;
    // print normalized orig_mat*vect, to verify eigen property
    double prod[4];
    for (int i=0; i<4; i++)
    {   double sum=0;
    	for (int j=0; j<4; j++)
    	{   sum += orig_mat[i][j]*vect[j];
	}
	prod[i] = sum;
    }
    normalize(prod);
    cerr << form("product      \t:  %7.4f %7.4f %7.4f %7.4f\n", 
	    prod[0], prod[1], prod[2], prod[3])
	    << flush;
#endif
}

Quaternion Optimal_rotation(const Pointlist &A, 
	const Pointlist &B, 
	const float * weight)
{
    
    if (A.num_points() ==0)
    {   Quaternion Q(1.0,0.0,0.0,0.0);	// null rotation ok for 0 points
	return Q;
    }
    double mat[4][4];
    double eigvector[4];

    Compute_SOP(mat,A,B,weight);
    eigen4(mat, eigvector);
    return static_cast<Quaternion>(eigvector);
}

// CHANGE LOG:
// 15 March 2004 Kevin Karplus
//	Fixed old-style casts
// Fri Sep  8 06:18:10 PDT 2006 Kevin Karplus
//	redid matrix copy as 16 assignments, rather than using reinterpret_cast
// Tue Jan  2 11:32:55 PST 2007 Kevin Karplus
//	Added minor speedup (if wt==0) in Compute_SOP
// Mon Apr 27 17:39:04 PDT 2009 Kevin Karplus
//	Added is_finite assertions for points in Compute_SOP