/*! \file eigen.c
 *
 * \author Gordon Kindlmann
 */

/*
 * COPYRIGHT (c) 2011 The Diderot Project (http://diderot-language.cs.uchicago.edu)
 * All rights reserved.
 */

/*
  Teem: Tools to process and visualize scientific data and images              
  Copyright (C) 2011, 2010, 2009, University of Chicago
  Copyright (C) 2008, 2007, 2006, 2005  Gordon Kindlmann
  Copyright (C) 2004, 2003, 2002, 2001, 2000, 1999, 1998  University of Utah

  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public License
  (LGPL) as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.
  The terms of redistributing and/or modifying this software also
  include exceptions to the LGPL that facilitate static linking.

  This library 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
  Lesser General Public License for more details.

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

#include <Diderot/diderot.h>

#define ROOT_TRIPLE 2           /* ell_cubic_root_triple */
#define ROOT_SINGLE_DOUBLE 3    /* ell_cubic_root_single_double */
#define ROOT_THREE 4            /* ell_cubic_root_three */

// from http://en.wikipedia.org/wiki/Square_root_of_3
#define M_SQRT3 1.732050807568877293527446341506

#define SUB(v,i)       (((Diderot_union3_t)(v)).r[i])

/*
** All the three given vectors span only a 2D space, and this finds
** the normal to that plane.  Simply sums up all the pair-wise
** cross-products to get a good estimate.  Trick is getting the cross
** products to line up before summing.
*/
STATIC_INLINE Diderot_vec3_t nullspace1 (
    const Diderot_vec3_t r0,
    const Diderot_vec3_t r1,
    const Diderot_vec3_t r2)
{
    Diderot_vec3_t ret, crs;
  
    ret = cross3(r0, r1);
    crs = cross3(r1, r2);
  /* ret += crs or ret -= crs; whichever makes ret longer */
    if (dot3(ret, crs) > 0.0)
	ret += crs;
    else
	ret -= crs;

    crs = cross3(r0, r2);
  /* ret += crs or ret -= crs; whichever makes ret longer */
    if (dot3(ret, crs) > 0.0)
	ret += crs;
    else
	ret -= crs;

    return ret;
}

/*
** All vectors are in the same 1D space, we have to find two 
** mutually vectors perpendicular to that span
*/
static void nullspace2 (Diderot_vec3_t rets[2],
    const Diderot_vec3_t r0,
    const Diderot_vec3_t r1,
    const Diderot_vec3_t r2)
{
    Diderot_vec3_t sqr, sum;
    int idx;

    sum = r0;
    if (dot3(sum, r1) > 0)
	sum += r1;
    else
	sum -= r1;
    if (dot3(sum, r2) > 0)
	sum += r2;
    else
	sum -= r2;
  // find largest component, to get most stable expression for a perpendicular vector
    sqr = sum*sum;
    idx = 0;
    if (SUB(sqr,0) < SUB(sqr,1))
	idx = 1;
    if (SUB(sqr, idx) < SUB(sqr,2))
	idx = 2;
  
    if (0 == idx) {
      rets[0] = vec3(SUB(sum,1) - SUB(sum,2), -SUB(sum,0), SUB(sum,0));
    } else if (1 == idx) {
      rets[0] = vec3(-SUB(sum,1), SUB(sum,0) - SUB(sum,2), SUB(sum,1));
    } else {
      rets[0] = vec3(-SUB(sum,2), SUB(sum,2), SUB(sum,0) - SUB(sum,1));
    }
  
    rets[1] = cross3(rets[0], sum);
    return;
}

/*
** Eigensolver for symmetric 3x3 matrix:
**
**  M00  M01  M02
**  M01  M11  M12
**  M02  M12  M22
**
** Must be passed pointer to eval vector, and will compute eigenvalues
** only if evec[9] is non-NULL.  Computed eigenvectors are at evec+0,
** evec+3, and evec+6.
**
** Return value indicates something about the eigenvalue solution to
** the cubic characteristic equation; see ROOT_ #defines above
** 
** HEY: the numerical precision issues here are very subtle, and
** merit some more scrutiny.  With evals (1.000001, 1, 1), for example,
** whether it comes back as a single/double root, vs three distinct roots,
** is determines by the comparison between "D" and "epsilon", and the 
** setting of epsilon seems pretty arbitrary at this point...
** 
*/
int Diderot_evals3x3 (
    Diderot_real_t eval[3],
    const Diderot_real_t _M00, const Diderot_real_t _M01, const Diderot_real_t _M02, 
    const Diderot_real_t _M11, const Diderot_real_t _M12, 
    const Diderot_real_t _M22)
{
    Diderot_real_t mean, norm, rnorm, Q, R, QQQ, D, theta, M00, M01, M02, M11, M12, M22;
// FIXME: probably want a different value of epsilon for single-precision
    Diderot_real_t epsilon = 1.0E-12;
    int roots;

  /* copy the given matrix elements */
    M00 = _M00;
    M01 = _M01;
    M02 = _M02;
    M11 = _M11;
    M12 = _M12;
    M22 = _M22;

  /*
  ** subtract out the eigenvalue mean (will add back to evals later);
  ** helps with numerical stability
  */
    mean = (M00 + M11 + M22)/3.0;
    M00 -= mean;
    M11 -= mean;
    M22 -= mean;
  
  /* 
  ** divide out L2 norm of eigenvalues (will multiply back later);
  ** this too seems to help with stability
  */
    norm = SQRT(M00*M00 + 2*M01*M01 + 2*M02*M02 +
		M11*M11 + 2*M12*M12 +
		M22*M22);
// QUESTION: what if norm is very close to zero?
    rnorm = norm ? 1.0/norm : 1.0;
    M00 *= rnorm;
    M01 *= rnorm;
    M02 *= rnorm;
    M11 *= rnorm;
    M12 *= rnorm;
    M22 *= rnorm;

  /* this code is a mix of prior Teem code and ideas from Eberly's
   * "Eigensystems for 3 x 3 Symmetric Matrices (Revisited)"
   */
    Q = (M01*M01 + M02*M02 + M12*M12 - M00*M11 - M00*M22 - M11*M22)/3.0;
    QQQ = Q*Q*Q;
    R = (M00*M11*M22 + M02*(2*M01*M12 - M02*M11) 
	 - M00*M12*M12 - M01*M01*M22)/2.0;
    D = QQQ - R*R;
    if (D > epsilon) {
      /* three distinct roots- this is the most common case */
	Diderot_real_t mm, ss, cc;
	theta = ATAN2(SQRT(D), R)/3.0;
	mm = SQRT(Q);
	ss = SIN(theta);
	cc = COS(theta);
	eval[0] = 2*mm*cc;
	eval[1] = mm*(-cc + M_SQRT3*ss);
	eval[2] = mm*(-cc - M_SQRT3*ss);
	roots = ROOT_THREE;
    }
  /* else D is near enough to zero */
    else if (R < -epsilon || epsilon < R) {
	Diderot_real_t U;
      /* one double root and one single root */
	U = CBRT(R); /* cube root function */
	if (U > 0) {
	    eval[0] = 2*U;
	    eval[1] = -U;
	    eval[2] = -U;
	}
	else {
	    eval[0] = -U;
	    eval[1] = -U;
	    eval[2] = 2*U;
	}
	roots = ROOT_SINGLE_DOUBLE;
    }
    else {
      /* a triple root! */
	eval[0] = eval[1] = eval[2] = 0.0;
	roots = ROOT_TRIPLE;
    }

  /* multiply back by eigenvalue L2 norm */
    eval[0] /= rnorm;
    eval[1] /= rnorm;
    eval[2] /= rnorm;

  /* add back in the eigenvalue mean */
    eval[0] += mean;
    eval[1] += mean;
    eval[2] += mean;

    return roots;
}

int Diderot_evecs3x3 (
	Diderot_real_t eval[3], Diderot_vec3_t evecs[3],
	const Diderot_real_t _M00, const Diderot_real_t _M01, const Diderot_real_t _M02, 
	const Diderot_real_t _M11, const Diderot_real_t _M12, 
	const Diderot_real_t _M22)
{
    Diderot_real_t len, dot;

/* BEGIN #include "teigen-evals-A.c" */
    Diderot_real_t mean, norm, rnorm, Q, R, QQQ, D, theta, M00, M01, M02, M11, M12, M22;
    Diderot_real_t epsilon = 1.0E-12;
    int roots;

  /* copy the given matrix elements */
    M00 = _M00;
    M01 = _M01;
    M02 = _M02;
    M11 = _M11;
    M12 = _M12;
    M22 = _M22;

  /*
  ** subtract out the eigenvalue mean (will add back to evals later);
  ** helps with numerical stability
  */
    mean = (M00 + M11 + M22)/3.0;
    M00 -= mean;
    M11 -= mean;
    M22 -= mean;
  
  /* 
  ** divide out L2 norm of eigenvalues (will multiply back later);
  ** this too seems to help with stability
  */
    norm = SQRT(M00*M00 + 2*M01*M01 + 2*M02*M02 +
                M11*M11 + 2*M12*M12 +
                M22*M22);
    rnorm = norm ? 1.0/norm : 1.0;
    M00 *= rnorm;
    M01 *= rnorm;
    M02 *= rnorm;
    M11 *= rnorm;
    M12 *= rnorm;
    M22 *= rnorm;

  /* this code is a mix of prior Teem code and ideas from Eberly's
   * "Eigensystems for 3 x 3 Symmetric Matrices (Revisited)"
   */
    Q = (M01*M01 + M02*M02 + M12*M12 - M00*M11 - M00*M22 - M11*M22)/3.0;
    QQQ = Q*Q*Q;
    R = (M00*M11*M22 + M02*(2*M01*M12 - M02*M11) - M00*M12*M12 - M01*M01*M22)/2.0;
    D = QQQ - R*R;
    if (D > epsilon) {
      /* three distinct roots- this is the most common case */
        Diderot_real_t mm, ss, cc;
        theta = ATAN2(SQRT(D), R)/3.0;
        mm = SQRT(Q);
        ss = SIN(theta);
        cc = COS(theta);
        eval[0] = 2*mm*cc;
        eval[1] = mm*(-cc + SQRT(3.0)*ss);
        eval[2] = mm*(-cc - SQRT(3.0)*ss);
        roots = ROOT_THREE;
    }
  /* else D is near enough to zero */
    else if (R < -epsilon || epsilon < R) {
        Diderot_real_t U;
      /* one double root and one single root */
        U = CBRT(R); /* cube root function */
        if (U > 0) {
            eval[0] = 2*U;
            eval[1] = -U;
            eval[2] = -U;
        } else {
            eval[0] = -U;
            eval[1] = -U;
            eval[2] = 2*U;
        }
        roots = ROOT_SINGLE_DOUBLE;
    }
    else {
      /* a triple root! */
        eval[0] = eval[1] = eval[2] = 0.0;
        roots = ROOT_TRIPLE;
    }
/* END #include "teigen-evals-A.c" */

    Diderot_vec3_t ev = vec3(eval[0], eval[1], eval[2]);
    if (ROOT_THREE == roots) {
	evecs[0] = nullspace1 (
		vec3(M00 - eval[0], M01, M02),
		vec3(M01, M11 - eval[0], M12),
		vec3(M02, M12, M22 - eval[0]));
	evecs[1] = nullspace1 (
		vec3(M00 - eval[1], M01, M02),
		vec3(M01, M11 - eval[1], M12),
		vec3(M02, M12, M22 - eval[1]));
	evecs[2] = nullspace1 (
		vec3(M00 - eval[2], M01, M02),
		vec3(M01, M11 - eval[2], M12),
		vec3(M02, M12, M22 - eval[2]));
    }
    else if (ROOT_SINGLE_DOUBLE == roots) {
	if (eval[1] == eval[2]) {
	  /* one big (eval[0]) , two small (eval[1,2]) */
	    evecs[0] = nullspace1 (
		    vec3(M00 - eval[0], M01, M02),
		    vec3(M01, M11 - eval[0], M12),
		    vec3(M02, M12, M22 - eval[0]));
	    nullspace2 (evecs+1,
		    vec3(M00 - eval[1], M01, M02),
		    vec3(M01, M11 - eval[1], M12),
		    vec3(M02, M12, M22 - eval[1]));
	}
	else {
	  /* two big (eval[0,1]), one small (eval[2]) */
	    nullspace2 (evecs,
		    vec3(M00 - eval[0], M01, M02),
		    vec3(M01, M11 - eval[0], M12),
		    vec3(M02, M12, M22 - eval[0]));
	    evecs[2] = nullspace1 (
		    vec3(M00 - eval[2], M01, M02),
		    vec3(M01, M11 - eval[2], M12),
		    vec3(M02, M12, M22 - eval[2]));
	}
    }
    else {
      /* ROOT_TRIPLE == roots; use any basis for eigenvectors */
	evecs[0] = vec3(1, 0, 0);
	evecs[1] = vec3(0, 1, 0);
	evecs[2] = vec3(0, 0, 1);
    }
  /* we always make sure it's really orthonormal; keeping fixed the
   * eigenvector associated with the largest-magnitude eigenvalue
   */
    if (ABS(eval[0]) > ABS(eval[2])) {
      /* normalize evec[0] but don't move it */
	evecs[0] = normalize3(evecs[0]);
	evecs[1] -= scale3(dot3(evecs[1], evecs[0]), evecs[0]);
	evecs[1] = normalize3(evecs[1]);
        evecs[2] = cross3(evecs[0], evecs[1]);
    }
    else {
      /* normalize evec[2] but don't move it */
	evecs[2] = normalize3(evecs[2]);
	evecs[1] -= scale3(dot3(evecs[1], evecs[2]), evecs[2]);
	evecs[1] = normalize3(evecs[1]);
        evecs[0] = cross3(evecs[1], evecs[2]);
    }
    /* note that the right-handedness check has been folded into
       the code above to enforce orthogonality.  Indeed, some work
       could be removed by never really bothering to find all three
       eigenvectors; just find two and then use the cross-product. 
       The next iteration of the code will do that */
    
/* BEGIN #include "teigen-evals-B.c" */
  /* multiply back by eigenvalue L2 norm */
    eval[0] /= rnorm;
    eval[1] /= rnorm;
    eval[2] /= rnorm;

  /* add back in the eigenvalue mean */
    eval[0] += mean;
    eval[1] += mean;
    eval[2] += mean;
/* END #include "teigen-evals-B.c" */

    return roots;
}


#if 0
/* command-line tester for above*/

#include <math.h>
#include <teem/hest.h>
#include <teem/nrrd.h>
#include <teem/ell.h>
char *info = ("tests symmetric 3x3 eigensolve.");

int
main(int argc, const char *argv[0]) {
  const char *me;
  hestOpt *hopt=NULL;
  airArray *mop;

  Nrrd *nbhess;
  float *bhess, mat[9];
  Diderot_real_t eval[3];
  Diderot_vec3_t evec[3];
  int roots, ri;

  mop = airMopNew();
  me = argv[0];
  hestOptAdd(&hopt, "i", "hess", airTypeOther, 1, 1, &nbhess, NULL,
             "bad hessian",
             NULL, NULL, nrrdHestNrrd);
  hestParseOrDie(hopt, argc-1, argv+1, NULL,
                 me, info, AIR_TRUE, AIR_TRUE, AIR_TRUE);
  airMopAdd(mop, hopt, (airMopper)hestOptFree, airMopAlways);
  airMopAdd(mop, hopt, (airMopper)hestParseFree, airMopAlways);

  bhess = AIR_CAST(float *, nbhess->data);
  roots = Diderot_evals3x3(eval, 
                           bhess[0], bhess[1], bhess[2],
                           bhess[4], bhess[5],
                           bhess[8]);
  printf("%s: roots(%d) = %g %g %g\n", me, roots, eval[0], eval[1], eval[2]);
  ELL_3V_SET(mat + 0, bhess[0], bhess[1], bhess[2]);
  ELL_3V_SET(mat + 3, bhess[3], bhess[4], bhess[5]);
  ELL_3V_SET(mat + 6, bhess[6], bhess[7], bhess[8]);
  printf("%s: det(H) = %g\n", me, ELL_3M_DET(mat));
  for (ri=0; ri<3; ri++) {
    ELL_3V_SET(mat + 0, bhess[0] - eval[ri], bhess[1], bhess[2]);
    ELL_3V_SET(mat + 3, bhess[3], bhess[4] - eval[ri], bhess[5]);
    ELL_3V_SET(mat + 6, bhess[6], bhess[7], bhess[8] - eval[ri]);
    printf("%s: det(H - lI) = %g\n", me, ELL_3M_DET(mat));
  }

  roots = Diderot_evecs3x3(eval, evec,
                           bhess[0], bhess[1], bhess[2],
                           bhess[4], bhess[5],
                           bhess[8]);
  printf("%s: roots(%d) = %g %g %g\n", me, roots, eval[0], eval[1], eval[2]);
  printf("%s: evec0 . evec0 = %g\n", me, ELL_3V_DOT(evec[0], evec[0]));
  printf("%s: evec1 . evec1 = %g\n", me, ELL_3V_DOT(evec[1], evec[1]));
  printf("%s: evec2 . evec2 = %g\n", me, ELL_3V_DOT(evec[2], evec[2]));
  printf("%s: evec0 . evec1 = %g\n", me, ELL_3V_DOT(evec[0], evec[1]));
  printf("%s: evec0 . evec2 = %g\n", me, ELL_3V_DOT(evec[0], evec[2]));
  printf("%s: evec1 . evec2 = %g\n", me, ELL_3V_DOT(evec[1], evec[2]));

  return 0;
}

/*

clang -m64 -I/Users/gk/diderot/diderot/branches/pure-cfg/src/include \
 -I../../include -I../../../../../../teem/include/ \
 -L../../../../../../teem/lib \
 -DDIDEROT_TARGET_C -DNDEBUG -DDIDEROT_SINGLE_PRECISION \
 -o eigen3 eigen3x3.c -lteem -lm

*/

#endif

<script type="text/javascript">//<![CDATA[
		function toggle_ffErrors() {
			var errorsblock = document.getElementById("ffErrorsBlock");
			var errorsgroup = document.getElementById("ffErrors");
			if (errorsblock.style.display == "none") {
				errorsblock.style.display = "block";
				errorsgroup.style.right = "10px";
			} else {
				errorsblock.style.display = "none";
				errorsgroup.style.right = "300px";
			}
		}
//]]></script>
<div id="ffErrors">
<a href="javascript:toggle_ffErrors();">Click to toggle</a>
<div id="ffErrorsBlock">
	<div class="error">does not end with &lt;/html&gt; tag</div>
	<div class="error">does not end with &lt;/body&gt; tag</div>
	<div class="info">The output has ended thus: DDIDEROT_TARGET_C -DNDEBUG -DDIDEROT_SINGLE_PRECISION \
 -o eigen3 eigen3x3.c -lteem -lm

*/

#endif</div>
</div></div>
</body></html>