#include "mex.h"
#include "math.h"

#ifdef MAX
#undef MAX
#endif
#define MAX(a, b) ((a)>(b)?(a):(b))
#define absd(a) ((a)>(-a)?(a):(-a))
#ifndef pow2
#define pow2(a) (a*a)
#endif
#define n 3
#define inv3 0.3333333333333333
#define root3 1.7320508075688772
/* This function
 *
 *
 *
 */

/* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
 * domain Java Matrix library JAMA. */
static double hypot2(double x, double y) { return sqrt(x*x+y*y); }

/* Symmetric Householder reduction to tridiagonal form. */
static void tred2(double V[n][n], double d[n], double e[n]) {
    
/*  This is derived from the Algol procedures tred2 by */
/*  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for */
/*  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding */
/*  Fortran subroutine in EISPACK. */
    int i, j, k;
    double scale;
    double f, g, h;
    double hh;
    for (j = 0; j < n; j++) {d[j] = V[n-1][j]; }
    
    /* Householder reduction to tridiagonal form. */
    
    for (i = n-1; i > 0; i--) {
        /* Scale to avoid under/overflow. */
        scale = 0.0;
        h = 0.0;
        for (k = 0; k < i; k++) { scale = scale + fabs(d[k]); }
        if (scale == 0.0) {
            e[i] = d[i-1];
            for (j = 0; j < i; j++) { d[j] = V[i-1][j]; V[i][j] = 0.0;  V[j][i] = 0.0; }
        } else {
            
            /* Generate Householder vector. */
            
            for (k = 0; k < i; k++) { d[k] /= scale; h += d[k] * d[k]; }
            f = d[i-1];
            g = sqrt(h);
            if (f > 0) { g = -g; }
            e[i] = scale * g;
            h = h - f * g;
            d[i-1] = f - g;
            for (j = 0; j < i; j++) { e[j] = 0.0; }
            
            /* Apply similarity transformation to remaining columns. */
            
            for (j = 0; j < i; j++) {
                f = d[j];
                V[j][i] = f;
                g = e[j] + V[j][j] * f;
                for (k = j+1; k <= i-1; k++) { g += V[k][j] * d[k]; e[k] += V[k][j] * f; }
                e[j] = g;
            }
            f = 0.0;
            for (j = 0; j < i; j++) { e[j] /= h; f += e[j] * d[j]; }
            hh = f / (h + h);
            for (j = 0; j < i; j++) { e[j] -= hh * d[j]; }
            for (j = 0; j < i; j++) {
                f = d[j]; g = e[j];
                for (k = j; k <= i-1; k++) { V[k][j] -= (f * e[k] + g * d[k]); }
                d[j] = V[i-1][j];
                V[i][j] = 0.0;
            }
        }
        d[i] = h;
    }
    
    /* Accumulate transformations. */
    
    for (i = 0; i < n-1; i++) {
        V[n-1][i] = V[i][i];
        V[i][i] = 1.0;
        h = d[i+1];
        if (h != 0.0) {
            for (k = 0; k <= i; k++) { d[k] = V[k][i+1] / h;}
            for (j = 0; j <= i; j++) {
                g = 0.0;
                for (k = 0; k <= i; k++) { g += V[k][i+1] * V[k][j]; }
                for (k = 0; k <= i; k++) { V[k][j] -= g * d[k]; }
            }
        }
        for (k = 0; k <= i; k++) { V[k][i+1] = 0.0;}
    }
    for (j = 0; j < n; j++) { d[j] = V[n-1][j]; V[n-1][j] = 0.0; }
    V[n-1][n-1] = 1.0;
    e[0] = 0.0;
}

/* Symmetric tridiagonal QL algorithm. */
static void tql2(double V[n][n], double d[n], double e[n]) {
    
/*  This is derived from the Algol procedures tql2, by */
/*  Bowdler, Martin, Reinsch, and Wilkinson, Handbook for */
/*  Auto. Comp., Vol.ii-Linear Algebra, and the corresponding */
/*  Fortran subroutine in EISPACK. */
    
    int i, j, k, l, m;
    double f;
    double tst1;
    double eps;
    int iter;
    double g, p, r;
    double dl1, h, c, c2, c3, el1, s, s2;
    
    for (i = 1; i < n; i++) { e[i-1] = e[i]; }
    e[n-1] = 0.0;
    
    f = 0.0;
    tst1 = 0.0;
    eps = pow(2.0, -52.0);
    for (l = 0; l < n; l++) {
        
        /* Find small subdiagonal element */
        
        tst1 = MAX(tst1, fabs(d[l]) + fabs(e[l]));
        m = l;
        while (m < n) {
            if (fabs(e[m]) <= eps*tst1) { break; }
            m++;
        }
        
        /* If m == l, d[l] is an eigenvalue, */
        /* otherwise, iterate. */
        
        if (m > l) {
            iter = 0;
            do {
                iter = iter + 1;  /* (Could check iteration count here.) */
                /* Compute implicit shift */
                g = d[l];
                p = (d[l+1] - g) / (2.0 * e[l]);
                r = hypot2(p, 1.0);
                if (p < 0) { r = -r; }
                d[l] = e[l] / (p + r);
                d[l+1] = e[l] * (p + r);
                dl1 = d[l+1];
                h = g - d[l];
                for (i = l+2; i < n; i++) { d[i] -= h; }
                f = f + h;
                /* Implicit QL transformation. */
                p = d[m]; c = 1.0; c2 = c; c3 = c;
                el1 = e[l+1]; s = 0.0; s2 = 0.0;
                for (i = m-1; i >= l; i--) {
                    c3 = c2;
                    c2 = c;
                    s2 = s;
                    g = c * e[i];
                    h = c * p;
                    r = hypot2(p, e[i]);
                    e[i+1] = s * r;
                    s = e[i] / r;
                    c = p / r;
                    p = c * d[i] - s * g;
                    d[i+1] = h + s * (c * g + s * d[i]);
                    /* Accumulate transformation. */
                    for (k = 0; k < n; k++) {
                        h = V[k][i+1];
                        V[k][i+1] = s * V[k][i] + c * h;
                        V[k][i] = c * V[k][i] - s * h;
                    }
                }
                p = -s * s2 * c3 * el1 * e[l] / dl1;
                e[l] = s * p;
                d[l] = c * p;
                
                /* Check for convergence. */
            } while (fabs(e[l]) > eps*tst1);
        }
        d[l] = d[l] + f;
        e[l] = 0.0;
    }
    
    /* Sort eigenvalues and corresponding vectors. */
    for (i = 0; i < n-1; i++) {
        k = i;
        p = d[i];
        for (j = i+1; j < n; j++) {
            if (d[j] < p) {
                k = j;
                p = d[j];
            }
        }
        if (k != i) {
            d[k] = d[i];
            d[i] = p;
            for (j = 0; j < n; j++) {
                p = V[j][i];
                V[j][i] = V[j][k];
                V[j][k] = p;
            }
        }
    }
}



void roots3(double d[3], double c0,double c1, double c2)
{
    double c2Div3, aDiv3, mbDiv2, q, magnitude, angle, cs, sn;
    
    /* Solve the roots of  y^3 + c2 * y^2 + c1 *y + c0  */
    c2Div3 = -c2*inv3;
    aDiv3 = (c1 + c2*c2Div3)*inv3;
    if (aDiv3 > 0.0) { aDiv3 = 0.0; }
    mbDiv2 = 0.5*(-c0 + c2Div3*(2.0*c2Div3*c2Div3 - c1));
    q = mbDiv2*mbDiv2 + aDiv3*aDiv3*aDiv3;
    if (q > 0.0) { q = 0.0; }
    magnitude = sqrt(-aDiv3);
    angle = atan2(sqrt(-q),mbDiv2)*inv3;
    cs = cos(angle);
    sn = sin(angle);
    d[0] = c2Div3 + 2.0*magnitude*cs;
    d[1] = c2Div3 - magnitude*(cs + root3*sn);
    d[2] = c2Div3 - magnitude*(cs - root3*sn);
}   

int fast_eigen3x3(double A[3][3], double V[3][3], double d[3])
{
    const double smallv=1e-12;
    double c0, c1,c2;
    int check;
	double l1, l2, l3;
    double t;
    double a1, a2, a3, b1, b2;
    double da[3];
    check=(absd(A[0][1])<smallv)+(absd(A[0][2])<smallv)+(absd(A[1][2])<smallv);
    if(check>1) { return 0; }

    /* 0 = - det (A - yI) = y^3 + c2 * y^2 + c1 *y + c0 */
    c0 = -(A[0][0]*A[1][1]*A[2][2] + 2*A[0][1]*A[0][2]*A[1][2] - A[0][0] * pow2(A[1][2]) - A[1][1]*pow2(A[0][2]) - A[2][2]*pow2(A[0][1]));
    c1 = A[0][0]*A[1][1] - pow2(A[0][1]) + A[0][0]*A[2][2] -pow2(A[0][2]) + A[1][1]*A[2][2] - pow2(A[1][2]);
    c2 = - (A[0][0] + A[1][1] + A[2][2]);

    /* Solve the roots of  y^3 + c2 * y^2 + c1 *y + c0  */
    roots3(d, c0, c1, c2);

    da[0]=absd(d[0]); da[1]=absd(d[1]); da[2]=absd(d[2]);
    /* Sort eigenvalues */
    if(da[0]>=da[1])
    {
        if(da[0]>da[2])
        {
            t=d[0]; d[0]=d[2]; d[2]=t; 
            t=da[0]; da[0]=da[2]; da[2]=t; 
        }
    }
    else if(da[1]>da[2])
    {
        t=d[1]; d[1]=d[2]; d[2]=t; 
        t=da[1]; da[1]=da[2]; da[2]=t; 

    }
    
    if(da[0]>=da[1])
    {
        t=d[0]; d[0]=d[1]; d[1]=t; 
        t=da[0]; da[0]=da[1]; da[1]=t; 
    }

    if((da[1]-da[0])<smallv) { return 0; }
    if((da[2]-da[1])<smallv) { return 0; }
    
    /* Calculate eigen vectors */
    a1=A[0][1]*A[1][2]; a2=A[0][1]*A[0][2]; a3=pow2(A[0][1]);

    b1=A[0][0]-d[0]; b2=A[1][1]-d[0];
    V[0][0]=a1-A[0][2]*b2; V[1][0]=a2-A[1][2]*b1; V[2][0]=b1*b2-a3;

    b1=A[0][0]-d[1]; b2=A[1][1]-d[1];
    V[0][1]=a1-A[0][2]*b2; V[1][1]=a2-A[1][2]*b1; V[2][1]=b1*b2-a3;

    b1=A[0][0]-d[2]; b2=A[1][1]-d[2];
    V[0][2]=a1-A[0][2]*b2; V[1][2]=a2-A[1][2]*b1; V[2][2]=b1*b2-a3;


    /* Eigen vector normalization */
    l1=sqrt(pow2(V[0][0])+ pow2(V[1][0]) + pow2(V[2][0]));
    l2=sqrt(pow2(V[0][1])+ pow2(V[1][1]) + pow2(V[2][1]));
    l3=sqrt(pow2(V[0][2])+ pow2(V[1][2]) + pow2(V[2][2]));

    /* Detect fail : eigenvectors with only zeros */
    if(l1<smallv) { return 0; }
    if(l2<smallv) {	return 0; }
    if(l3<smallv) { return 0; }

    V[0][0]/=l1; V[0][1]/=l2; V[0][2]/=l3;
    V[1][0]/=l1; V[1][1]/=l2; V[1][2]/=l3;
    V[2][0]/=l1; V[2][1]/=l2; V[2][2]/=l3;
    
    /* Succes    */
    return 1;
}

void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) {
    double e[n];
    double da[3];
    double dt, dat;
    double vet[3];
    int i, j;
    
    if(fast_eigen3x3(A, V, d)) { return; }
    for (i = 0; i < n; i++) {
        for (j = 0; j < n; j++) {
            V[i][j] = A[i][j];
        }
    }
    tred2(V, d, e);
    tql2(V, d, e);
    
    /* Sort the eigen values and vectors by abs eigen value */
    da[0]=absd(d[0]); da[1]=absd(d[1]); da[2]=absd(d[2]);
    if((da[0]>=da[1])&&(da[0]>da[2]))
    {
        dt=d[2];   dat=da[2];    vet[0]=V[0][2];    vet[1]=V[1][2];    vet[2]=V[2][2];
        d[2]=d[0]; da[2]=da[0];  V[0][2] = V[0][0]; V[1][2] = V[1][0]; V[2][2] = V[2][0];
        d[0]=dt;   da[0]=dat;    V[0][0] = vet[0];  V[1][0] = vet[1];  V[2][0] = vet[2]; 
    }
    else if((da[1]>=da[0])&&(da[1]>da[2]))  
    {
        dt=d[2];   dat=da[2];    vet[0]=V[0][2];    vet[1]=V[1][2];    vet[2]=V[2][2];
        d[2]=d[1]; da[2]=da[1];  V[0][2] = V[0][1]; V[1][2] = V[1][1]; V[2][2] = V[2][1];
        d[1]=dt;   da[1]=dat;    V[0][1] = vet[0];  V[1][1] = vet[1];  V[2][1] = vet[2]; 
    }
    if(da[0]>da[1])
    {
        dt=d[1];   dat=da[1];    vet[0]=V[0][1];    vet[1]=V[1][1];    vet[2]=V[2][1];
        d[1]=d[0]; da[1]=da[0];  V[0][1] = V[0][0]; V[1][1] = V[1][0]; V[2][1] = V[2][0];
        d[0]=dt;   da[0]=dat;    V[0][0] = vet[0];  V[1][0] = vet[1];  V[2][0] = vet[2]; 
    }
}