/*! \file bmark-teem.c
 *
 * \author Gordon Kindlmann
 */

#include <stdio.h>
#include <math.h>
#include "teem/nrrd.h"
#include "teem/gage.h"
#include "teem/ell.h"

extern int _gageProbe(gageContext *ctx, double _xi, double _yi, double _zi, double _si);

#include "teem-defs.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 */

#define ABS(a) (((a) > 0.0f ? (a) : -(a)))
#define VEC_SET(v, a, b, c) \
  ((v)[0] = (a), (v)[1] = (b), (v)[2] = (c))
#define VEC_DOT(v1, v2) \
  ((v1)[0]*(v2)[0] + (v1)[1]*(v2)[1] + (v1)[2]*(v2)[2])
#define VEC_CROSS(v3, v1, v2) \
  ((v3)[0] = (v1)[1]*(v2)[2] - (v1)[2]*(v2)[1], \
   (v3)[1] = (v1)[2]*(v2)[0] - (v1)[0]*(v2)[2], \
   (v3)[2] = (v1)[0]*(v2)[1] - (v1)[1]*(v2)[0])
#define VEC_ADD(v1, v2)  \
  ((v1)[0] += (v2)[0], \
   (v1)[1] += (v2)[1], \
   (v1)[2] += (v2)[2])
#define VEC_SUB(v1, v2)  \
  ((v1)[0] -= (v2)[0], \
   (v1)[1] -= (v2)[1], \
   (v1)[2] -= (v2)[2])
#define VEC_SCL(v1, s)  \
  ((v1)[0] *= (s),      \
   (v1)[1] *= (s),      \
   (v1)[2] *= (s))
#define VEC_LEN(v) (SQRT(VEC_DOT(v,v)))
#define VEC_NORM(v, len) ((len) = VEC_LEN(v), VEC_SCL(v, 1.0/len))
#define VEC_SCL_SUB(v1, s, v2) \
  ((v1)[0] -= (s)*(v2)[0],      \
   (v1)[1] -= (s)*(v2)[1], \
   (v1)[2] -= (s)*(v2)[2])
#define VEC_COPY(v1, v2)  \
  ((v1)[0] = (v2)[0], \
   (v1)[1] = (v2)[1], \
   (v1)[2] = (v2)[2])

#if 1
#  define REAL  float
#  define ATAN2 atan2f
#  define SQRT  sqrtf
#  define COS   cosf
#  define SIN   sinf
#else
#  define REAL  double
#  define ATAN2 atan2
#  define SQRT  sqrt
#  define COS   cos
#  define SIN   sin
#endif

/*
** 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 void nullspace1(REAL ret[3],  const REAL r0[3], const REAL r1[3], const REAL r2[3])
{
  REAL crs[3];
  
  /* ret = r0 x r1 */
  VEC_CROSS(ret, r0, r1);
  /* crs = r1 x r2 */
  VEC_CROSS(crs, r1, r2);
  /* ret += crs or ret -= crs; whichever makes ret longer */
  if (VEC_DOT(ret, crs) > 0) {
    VEC_ADD(ret, crs);
  } else {
    VEC_SUB(ret, crs);
  }
  /* crs = r0 x r2 */
  VEC_CROSS(crs, r0, r2);
  /* ret += crs or ret -= crs; whichever makes ret longer */
  if (VEC_DOT(ret, crs) > 0) {
    VEC_ADD(ret, crs);
  } else {
    VEC_SUB(ret, crs);
  }

  return;
}

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

  VEC_COPY(sum, r0);
  if (VEC_DOT(sum, r1) > 0) {
    VEC_ADD(sum, r1);
  } else {
    VEC_SUB(sum, r1);
  }
  if (VEC_DOT(sum, r2) > 0) {
    VEC_ADD(sum, r2);
  } else {
    VEC_SUB(sum, r2);
  }
  /* find largest component, to get most stable expression for a 
     perpendicular vector */
  sqr[0] = sum[0]*sum[0];
  sqr[1] = sum[1]*sum[1];
  sqr[2] = sum[2]*sum[2];
  idx = 0;
  if (sqr[0] < sqr[1])
    idx = 1;
  if (sqr[idx] < sqr[2])
    idx = 2;
  /* reta will be perpendicular to sum */
  if (0 == idx) {
    VEC_SET(reta, sum[1] - sum[2], -sum[0], sum[0]);
  } else if (1 == idx) {
    VEC_SET(reta, -sum[1], sum[0] - sum[2], sum[1]);
  } else {
    VEC_SET(reta, -sum[2], sum[2], sum[0] - sum[1]);
  }
  /* and now retb will be perpendicular to both reta and sum */
  VEC_CROSS(retb, reta, sum);
  return;
}

int
evals_evecs(REAL eval[3], REAL evec[9],
            const REAL _M00, const REAL _M01, const REAL _M02, 
            const REAL _M11, const REAL _M12, 
            const REAL _M22) {
  REAL r0[3], r1[3], r2[3], crs[3], len, dot;

  REAL mean, norm, rnorm, Q, R, QQQ, D, theta,
    M00, M01, M02, M11, M12, M22;
  REAL 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 */
    REAL 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) {
    REAL U;
    /* one double root and one single root */
    U = airCbrt(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;
  }

  /* r0, r1, r2 are the vectors we manipulate to 
     find the nullspaces of M - lambda*I */
  VEC_SET(r0, 0.0, M01, M02);
  VEC_SET(r1, M01, 0.0, M12);
  VEC_SET(r2, M02, M12, 0.0);
  if (ROOT_THREE == roots) {
    r0[0] = M00 - eval[0]; r1[1] = M11 - eval[0]; r2[2] = M22 - eval[0];
    nullspace1(evec+0, r0, r1, r2);
    r0[0] = M00 - eval[1]; r1[1] = M11 - eval[1]; r2[2] = M22 - eval[1];
    nullspace1(evec+3, r0, r1, r2);
    r0[0] = M00 - eval[2]; r1[1] = M11 - eval[2]; r2[2] = M22 - eval[2];
    nullspace1(evec+6, r0, r1, r2);
  } else if (ROOT_SINGLE_DOUBLE == roots) {
    if (eval[1] == eval[2]) {
      /* one big (eval[0]) , two small (eval[1,2]) */
      r0[0] = M00 - eval[0]; r1[1] = M11 - eval[0]; r2[2] = M22 - eval[0];
      nullspace1(evec+0, r0, r1, r2);
      r0[0] = M00 - eval[1]; r1[1] = M11 - eval[1]; r2[2] = M22 - eval[1];
      nullspace2(evec+3, evec+6, r0, r1, r2);
    }
    else {
      /* two big (eval[0,1]), one small (eval[2]) */
      r0[0] = M00 - eval[0]; r1[1] = M11 - eval[0]; r2[2] = M22 - eval[0];
      nullspace2(evec+0, evec+3, r0, r1, r2);
      r0[0] = M00 - eval[2]; r1[1] = M11 - eval[2]; r2[2] = M22 - eval[2];
      nullspace1(evec+6, r0, r1, r2);
    }
  } else {
    /* ROOT_TRIPLE == roots; use any basis for eigenvectors */
    VEC_SET(evec+0, 1, 0, 0);
    VEC_SET(evec+3, 0, 1, 0);
    VEC_SET(evec+6, 0, 0, 1);
  }
  /* we always make sure its 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 */
    VEC_NORM(evec+0, len);
    dot = VEC_DOT(evec+0, evec+3); VEC_SCL_SUB(evec+3, dot, evec+0);
    VEC_NORM(evec+3, len);
    ELL_3V_CROSS(evec+6, evec+0, evec+3);
  } else {
    /* normalize evec+6 but don't move it */
    VEC_NORM(evec+6, len);
    dot = VEC_DOT(evec+6, evec+3); VEC_SCL_SUB(evec+3, dot, evec+6);
    VEC_NORM(evec+3, len);
    ELL_3V_CROSS(evec+0, evec+3, evec+6);
  }

  /* 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 main (int argc, const char *argv[])
{
  hestOpt *hopt=NULL;
  hestParm *hparm;
  airArray *mop;
  const char *me;

#define DATA_IN_STR "../../data/lungcrop.nrrd"
  unsigned int gridPoints, stepsMax;
  Nrrd *nin, *nout, *npos;
  char *err, *outS;
  float epsilon;
  double travelMax, strnMin;

  mop = airMopNew();
  hparm = hestParmNew();
  airMopAdd(mop, hparm, (airMopper)hestParmFree, airMopAlways);
  nout = nrrdNew();
  airMopAdd(mop, nout, (airMopper)nrrdNuke, airMopAlways);

  hparm->noArgsIsNoProblem = AIR_TRUE;
  me = argv[0];
  hestOptAdd(&hopt, "gridSize", "grid points", airTypeUInt, 1, 1,
             &gridPoints, "120",
             "number of points along volume edge");
  hestOptAdd(&hopt, "stepsMax", "# iters", airTypeUInt, 1, 1,
             &stepsMax, "30",
             "# iterations allowed for feature detection");
  hestOptAdd(&hopt, "epsilon", "convg", airTypeFloat, 1, 1,
             &epsilon, "0.001", "convergence threshold");
  hestOptAdd(&hopt, "travelMax", "tmax", airTypeDouble, 1, 1,
             &travelMax, "5.0", "max on travel");
  hestOptAdd(&hopt, "strnMin", "smin", airTypeDouble, 1, 1,
             &strnMin, "150", "minimum significant features strength");
  hestOptAdd(&hopt, "o", "out", airTypeString, 1, 1,
             &outS, "ridge-pos.nrrd", "output filename");

  hestParseOrDie(hopt, argc-1, argv+1, hparm,
                 me, "ridge3d", AIR_TRUE, AIR_TRUE, AIR_TRUE);
  airMopAdd(mop, hopt, (airMopper)hestOptFree, airMopAlways);
  airMopAdd(mop, hopt, (airMopper)hestParseFree, airMopAlways);

  nin = nrrdNew();
  airMopAdd(mop, nin, (airMopper)nrrdNuke, airMopAlways);
  if (nrrdLoad(nin, DATA_IN_STR, NULL)) {
    airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
    fprintf(stderr, "Trouble reading:\n%s", err);
    airMopError(mop); 
    return 1;
  }

  gageContext *ctx;
  gagePerVolume *pvl;
  double kparm[NRRD_KERNEL_PARMS_NUM] = {0.0};
  ctx = gageContextNew();
  airMopAdd(mop, ctx, (airMopper)gageContextNix, airMopAlways);
  pvl = gagePerVolumeNew(ctx, nin, gageKindScl);
  if (gagePerVolumeAttach(ctx, pvl)
      || gageKernelSet(ctx, gageKernel00, nrrdKernelBSpline3, kparm)
      || gageKernelSet(ctx, gageKernel11, nrrdKernelBSpline3D, kparm)
      || gageKernelSet(ctx, gageKernel22, nrrdKernelBSpline3DD, kparm)
      || gageQueryItemOn(ctx, pvl, gageSclGradVec)
      || gageQueryItemOn(ctx, pvl, gageSclHessian)
      || gageUpdate(ctx)) {
    airMopAdd(mop, err = biffGetDone(GAGE), airFree, airMopAlways);
    fprintf(stderr, "%s: Trouble setting up gage context:\n%s", me, err);
    airMopError(mop); 
    return 1;
  }

  npos = nrrdNew();
  airMopAdd(mop, npos, (airMopper)nrrdNuke, airMopAlways);
  if (nrrdAlloc_va(npos, nrrdTypeFloat, 2, 
                   AIR_CAST(size_t, 3),
                   AIR_CAST(size_t, gridPoints*gridPoints*gridPoints))) {
    airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
    fprintf(stderr, "%s: Trouble setting up nrrd output:\n%s", me, err);
    airMopError(mop); 
    return 1;
  }
  float *pos = AIR_CAST(float *, npos->data);

  int ui, vi, wi, si;
  float xx, yy, zz, wpos[4], ipos[4];
  const double *grad, *hess;
  REAL eval[3], evec[9];
  unsigned int posIdx = 0;
  if (!( (grad = gageAnswerPointer(ctx, pvl, gageSclGradVec)) 
         && (hess = gageAnswerPointer(ctx, pvl, gageSclHessian))
         )) {
    fprintf(stderr, "%s: got null answer pointers", me);
    airMopError(mop); 
    return 1;
  }
  wpos[3] = 1.0;
  /*
  printf("%s: hit return\n", me);
  fgetc(stdin);
  printf("%s: here we go\n", me);
  */

  double t0 = airTime(); // start timing

  for (wi=0; wi<gridPoints; wi++) {
    zz = AIR_AFFINE(-0.5, wi, gridPoints-0.5, -188.0000, -188.0000 + 140.0*0.5); /* HEY */
    for (vi=0; vi<gridPoints; vi++) {
      yy = AIR_AFFINE(-0.5, vi, gridPoints-0.5, -175.8792, -175.8792 + 200.0*0.5); /* HEY */
      for (ui=0; ui<gridPoints; ui++) {
        xx = AIR_AFFINE(-0.5, ui, gridPoints-0.5, 21.6401, 21.6401 + 140.0*0.5); /* HEY */
        ELL_3V_SET(wpos, xx, yy, zz);
        
        REAL travel = 0.0, proj1[9], proj2[9], proj[9], 
          dir[3], len, fdd, sdd, del;

#define PROBE(worldPos)                                              \
        ELL_4MV_MUL(ipos, ctx->shape->WtoI, worldPos);               \
        if ( ipos[0] <= 2 || ipos[0] >= 137                          \
             || ipos[1] <= 2 || ipos[1] >= 197                       \
             || ipos[2] <= 2 || ipos[2] >= 137 ) {                   \
          /* if not inside */                                        \
          break;                                                     \
        }                                                            \
        _gageProbe(ctx, ipos[0], ipos[1], ipos[2], 0.0);             \
        evals_evecs(eval, evec,                                      \
                    hess[0], hess[1], hess[2],                       \
                    hess[4], hess[5],                                \
                    hess[8])

        for (si=0; si<stepsMax; si++) {
          if (travel > travelMax) {
            break;
          }
          PROBE(wpos);
          if (!ELL_3V_LEN(grad)) {
            break;
          }
          ELL_3MV_OUTER(proj1, evec + 1*3, evec + 1*3);
          ELL_3MV_OUTER(proj2, evec + 2*3, evec + 2*3);
          ELL_3M_ADD2(proj, proj1, proj2);
          ELL_3MV_MUL(dir, proj, grad);
          VEC_NORM(dir, len);
          if (!len) {
            break;
          }
          fdd = ELL_3V_DOT(grad, dir);
          sdd = ELL_3MV_CONTR(hess, dir);
          del = sdd >= 0 ? 0.1 : -fdd/sdd;
          if (del < epsilon) {
            if (-eval[1] >= strnMin) {
              /* converged! save point */
              ELL_3V_COPY(pos + 3*posIdx, wpos);
              posIdx++;
              break;
            }
            break;
          }
          ELL_3V_SCALE_INCR(wpos, del, dir);
          travel += del;
        }
      }
    }
  }
  /* update number of actual output points */
  npos->axis[1].size = posIdx;

  double totalTime = airTime() - t0;  // report timing
  printf("usr=%f\n", totalTime);

  if (nrrdSave(outS, npos, NULL)) {
    airMopAdd(mop, err = biffGetDone(NRRD), airFree, airMopAlways);
    fprintf(stderr, "%s: Trouble saving output:\n%s", me, err);
    airMopError(mop); 
    return 1;
  }

  airMopOkay(mop);
  return 0;
}

<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: g output:\n%s&quot;, me, err);
    airMopError(mop); 
    return 1;
  }

  airMopOkay(mop);
  return 0;
}</div>
</div></div>
</body></html>