/* VectorNav HSI Calibration Library v1.0.0.2
 *
 * Copyright (c) 2018 VectorNav Technologies, LLC
 *
 * This source code is proprietary to and copyrighted by VectorNav Technologies, LLC and is
 * available solely under license from VectorNav. All rights reserved. If you do not have a
 * license to use this source code from VectorNav, any use hereof is strictly prohibited by
 * U.S. law and other applicable law. This source code is restricted for use solely with the
 * products of VectorNav and as otherwise set forth in any agreement with VectorNav. Any
 * disclosure of this source code to the public or to any third party is also prohibited.
 */
#include "vnhsimath.h"

#ifdef USE_COMPLEXH
  #include <complex.h>
#else
	float vncabs(vncomplex val)
	{
		return sqrtf(val.Re*val.Re + val.Im*val.Im);
	}
	vncomplex cmult(vncomplex v1, vncomplex v2)
	{
		vncomplex out;
		
		out.Re = v1.Re*v2.Re - v1.Im*v2.Im;
		out.Im = v1.Im*v2.Re + v1.Re*v2.Im;
		
		return out;
	}
	vncomplex cdiv(vncomplex num, vncomplex den)
	{
		vncomplex out;
		vncomplex tmp1 = den;
		
		float denmag = vncabs(den);
		
		tmp1.Im *= -1.0;
		out = cmult(num,tmp1);
		
		out.Re /= (denmag*denmag);
		out.Im /= (denmag*denmag);
		
		return out;
	}
#endif

static float pythag(float a, float b);

/* ------------------------- Calculate the eigenvalues and eigenvectors ------------------------ */

static void *vminit(void)
{
	return (vmblock_t*)calloc(1, sizeof(vmblock_t));
}

static int vmcomplete(void *vmblock)
{
	return 1;
}

static void vmfree(void *vmblock)
{
	vmblock_t *vmb = (vmblock_t*)vmblock;
	free(vmb->mem); free(vmblock);
}

static void *vmalloc(void *vmblock, int typ, size_t zeilen, size_t spalten)
{
	vmblock_t *vmb = (vmblock_t*)vmblock;
	float *ret = 0;
	if (typ == 0) {
		if (vmb->n + zeilen > vmb->max) {
			vmb->max = vmb->n + zeilen;
			vmb->mem = (float*)realloc(vmb->mem, vmb->max * sizeof(float));
		}
		ret = vmb->mem + vmb->n;
		vmb->n += zeilen;
	}
	return ret;
}

static int basis(void) /* find basis used for computer number representation */

{
  float x,
       eins,
       b;

  x = eins = b = ONE;

  while ((x + eins) - x == eins)
    x *= TWO;
  while ((x + b) == x)
    b *= TWO;


  return (int)((x + b) - x);
}
         
static int balance 
                   (int       n,      /* size of matrix ..............*/
                    float *    mat[],  /* matrix ......................*/
                    float      scal[], /* Scaling data ................*/
                    int *     low,    /* first relevant row index ....*/
                    int *     high,   /* last relevant row index .....*/
                    int       basis   /* base of computer numbers ....*/
                   )
/*====================================================================*
 *                                                                    *
 *  balance balances the matrix mat so that the rows with zero entries*
 *  off the diagonal are isolated and the remaining columns and rows  *
 *  are resized to have one norm close to 1.                          *
 *                                                                    *
 *====================================================================*/
{
  register int i, j;
  int      iter, k, m;
  float     b2, r, c, f, g, s;

  b2 = (float) (basis * basis);
  m = 0;
  k = n - 1;

  do
  {
    iter = FALSE;
    for (j = k; j >= 0; j--)
    {
      for (r = ZERO, i = 0; i <= k; i++)
        if (i != j)  r += ABS (mat[j][i]);

      if (r == ZERO)
      {
        scal[k] = (float) j;
        if (j != k)
        {
          for (i = 0; i <= k; i++) SWAP (float, mat[i][j], mat[i][k])
          for (i = m; i < n; i++)  SWAP (float, mat[j][i], mat[k][i])
        }
        k--;
        iter = TRUE;
      }
    }   /* end of j */
  }   /* end of do  */
  while (iter);

  do
  {
    iter = FALSE;
    for (j = m; j <= k; j++)
    {
      for (c = ZERO, i = m; i <= k; i++)
        if (i != j) c += ABS (mat[i][j]);
      if (c == ZERO)
      {
        scal[m] = (float) j;
        if ( j != m )
        {
          for (i = 0; i <= k; i++) SWAP (float, mat[i][j], mat[i][m])
          for (i = m; i < n; i++)  SWAP (float, mat[j][i], mat[m][i])
        }
        m++;
        iter = TRUE;
      }
    }   /* end of j */
  }   /* end of do  */
  while (iter);

  *low = m;
  *high = k;
  for (i = m; i <= k; i++) scal[i] = ONE;

  do
  {
    iter = FALSE;
    for (i = m; i <= k; i++)
    {
      for (c = r = ZERO, j = m; j <= k; j++)
      if (j !=i)
      {
        c += ABS (mat[j][i]);
        r += ABS (mat[i][j]);
      }
      g = r / basis;
      f = ONE;
      s = c + r;

      while (c < g)
      {
        f *= basis;
        c *= b2;
      }

      g = r * basis;
      while (c >= g)
      {
        f /= basis;
        c /= b2;
      }

      if ((c + r) / f < (float)0.95 * s)
      {
        g = ONE / f;
        scal[i] *= f;
        iter = TRUE;
        for (j = m; j < n; j++ ) mat[i][j] *= g;
        for (j = 0; j <= k; j++ ) mat[j][i] *= f;
      }
    }
  }
  while (iter);

  return (0);
}


static int balback       /* reverse balancing ........................*/
/*.IX{balback}*/
                   (int     n,        /* Dimension of matrix .........*/
                    int     low,      /* first nonzero row ...........*/
                    int     high,     /* last nonzero row ............*/
                    float    scal[],   /* Scaling data ................*/
                    float *  eivec[]   /* Eigenvectors ................*/
                   )
/*====================================================================*
 *                                                                    *
 *  balback reverses the balancing of balance for the eigenvactors.   *
 *====================================================================*/
{
  register int i, j, k;
  float s;

  for (i = low; i <= high; i++)
  {
    s = scal[i];
    for (j = 0; j < n; j++) eivec[i][j] *= s;
  }

  for (i = low - 1; i >= 0; i--)
  {
    k = (int) scal[i];
    if (k != i)
      for (j = 0; j < n; j++) SWAP (float, eivec[i][j], eivec[k][j])
  }

  for (i = high + 1; i < n; i++)
  {
    k = (int) scal[i];
    if (k != i)
      for (j = 0; j < n; j++) SWAP (float, eivec[i][j], eivec[k][j])
  }
  return (0);
}



static int elmhes       /* reduce matrix to upper Hessenberg form ....*/
/*.IX{elmhes}*/
                  (int       n,       /* Dimension of matrix .........*/
                   int       low,     /* first nonzero row ...........*/
                   int       high,    /* last nonzero row ............*/
                   float *    mat[],   /* input/output matrix .........*/
                   int       perm[]   /* Permutation vector ..........*/
                  )
/*====================================================================*
  *  elmhes transforms the matrix mat to upper Hessenberg form.        *
  *====================================================================*/
{
  register int i, j, m;
  float   x, y;

  for (m = low + 1; m < high; m++)
  {
    i = m;
    x = ZERO;
    for (j = m; j <= high; j++)
      if (ABS (mat[j][m-1]) > ABS (x))
      {
        x = mat[j][m-1];
        i = j;
      }

    perm[m] = i;
    if (i != m)
    {
      for (j = m - 1; j < n; j++) SWAP (float, mat[i][j], mat[m][j])
      for (j = 0; j <= high; j++) SWAP (float, mat[j][i], mat[j][m])
    }

    if (x != ZERO)
    {
      for (i = m + 1; i <= high; i++)
      {
        y = mat[i][m-1];
        if (y != ZERO)
        {
          y = mat[i][m-1] = y / x;
          for (j = m; j < n; j++) mat[i][j] -= y * mat[m][j];
          for (j = 0; j <= high; j++) mat[j][m] += y * mat[j][i];
        }
      } /* end i */
    }
  } /* end m */

  return (0);
}


static int elmtrans       /* copy to Hessenberg form .................*/
/*.IX{elmtrans}*/
                    (int     n,       /* Dimension of matrix .........*/
                     int     low,     /* first nonzero row ...........*/
                     int     high,    /* last nonzero row ............*/
                     float *  mat[],   /* input matrix ................*/
                     int     perm[],  /* row permutations ............*/
                     float *  h[]      /* Hessenberg matrix ...........*/
                    )
/*====================================================================*
 *  elmtrans copies the Hessenberg matrix stored in mat to h.         *
 *====================================================================*/
{
  register int k, i, j;

  for (i = 0; i < n; i++)
  {
    for (k = 0; k < n; k++) h[i][k] = ZERO;
    h[i][i] = ONE;
  }

  for (i = high - 1; i > low; i--)
  {
    j = perm[i];
    for (k = i + 1; k <= high; k++) h[k][i] = mat[k][i-1];
    if ( i != j )
    {
      for (k = i; k <= high; k++)
      {
        h[i][k] = h[j][k];
        h[j][k] = ZERO;
      }
      h[j][i] = ONE;
    }
  }

  return (0);
}


/* ------------------------------------------------------------------ */

static int orthes     /* reduce orthogonally to upper Hessenberg form */
/*.IX{orthes}*/
                 (
                  int  n,                  /* Dimension of matrix     */
                  int  low,                /* [low,low]..[high,high]: */
                  int  high,               /* submatrix to be reduced */
                  float *mat[],             /* input/output matrix     */
                  float d[]                 /* reduction information   */
                 )                         /* error code              */

/***********************************************************************
* This function reduces matrix mat to upper Hessenberg form by         *
* Householder transformations. All details of the transformations are  *
* stored in the remaining triangle of the Hessenberg matrix and in     *
* vector d.                                                            *
***********************************************************************/

{
  int  i, j, m;    /* loop variables                                  */
  float s,          /* Euclidian norm sigma of the subdiagonal column  */
                   /* vector v of mat, that shall be reflected into a */
                   /* multiple of the unit vector e1 = (1,0,...,0)    */
                   /* (v = (v1,..,v(high-m+1))                        */
       x = ZERO,   /* first element of v in the beginning, then       */
                   /* summation variable in the actual Householder    */
                   /* transformation                                  */
       y,          /* sigma^2 in the beginning, then ||u||^2, with    */
                   /* u := v +- sigma * e1                            */
       eps;        /* tolerance for checking if the transformation is */
                   /* valid                                           */

  eps = (float)128.0 * MACH_EPS;

  for (m = low + 1; m < high; m++)
  {
    for (y = ZERO, i = high; i >= m; i--)
      x    = mat[i][m - 1],
      d[i] = x,
      y    = y + x * x;
    if (y <= eps)
      s = ZERO;
    else
    {
      s = (x >= ZERO) ? -sqrtf(y) : sqrtf(y);
      y    -= x * s;
      d[m] =  x - s;

      for (j = m; j < n; j++)               /* multiply mat from the  */
      {                                     /* left by  (E-(u*uT)/y)  */
        for (x = ZERO, i = high; i >= m; i--)
          x += d[i] * mat[i][j];
        for (x /= y, i = m; i <= high; i++)
          mat[i][j] -= x * d[i];
      }

      for (i = 0; i <= high; i++)           /* multiply mat from the  */
      {                                     /* right by  (E-(u*uT)/y) */
        for (x = ZERO, j = high; j >= m; j--)
          x += d[j] * mat[i][j];
        for (x /= y, j = m; j <= high; j++)
          mat[i][j] -= x * d[j];
      }
    }

    mat[m][m - 1] = s;
  }

  return 0;

}    

static int orttrans       /* compute orthogonal transformation matrix */
/*.IX{orttrans}*/
                   (
                    int  n,      /* Dimension of matrix               */
                    int  low,    /* [low,low]..[high,high]: submatrix */
                    int  high,   /* affected by the reduction         */
                    float *mat[], /* Hessenberg matrix, reduction inf. */
                    float d[],    /* remaining reduction information   */
                    float *v[]    /* transformation matrix             */
                   )             /* error code                        */

/***********************************************************************
* compute the matrix v of accumulated transformations from the         *
* information left by the Householder reduction of matrix mat to upper *
* Hessenberg form below the Hessenberg matrix in mat and in the        *
* vector d. The contents of the latter are destroyed.                  *
***********************************************************************/

{
  int  i, j, m;                        /* loop variables              */
  float x,                              /* summation variable in the   */
                                       /* Householder transformation  */
       y;                              /* sigma  respectively         */
                                       /* sigma * (v1 +- sigma)       */

  for (i = 0; i < n; i++)              /* form the unit matrix in v   */
  {
    for (j = 0; j < n; j++)
      v[i][j] = ZERO;
    v[i][i] = ONE;
  }

  for (m = high - 1; m > low; m--)     /* apply the transformations   */
  {                                    /* that reduced mat to upper   */
    y = mat[m][m - 1];                 /* Hessenberg form also to the */
                                       /* unit matrix in v. This      */
    if (y != ZERO)                     /* produces the desired        */
    {                                  /* transformation matrix in v. */
      y *= d[m];
      for (i = m + 1; i <= high; i++)
        d[i] = mat[i][m - 1];
      for (j = m; j <= high; j++)
      {
        for (x = ZERO, i = m; i <= high; i++)
          x += d[i] * v[i][j];
        for (x /= y, i = m; i <= high; i++)
          v[i][j] += x * d[i];
      }
    }
  }

  return 0;

}    /* -------------------------- orttrans ------------------------- */


/*#if NOT_COMPILING_IN_VISUAL_STUDIO*/

static int hqrvec       /* compute eigenvectors ......................*/
/*.IX{hqrvec}*/
                  (int     n,           /* Dimension of matrix .......*/
                   int     low,         /* first nonzero row .........*/
                   int     high,        /* last nonzero row ..........*/
                   float *  h[],         /* upper Hessenberg matrix ...*/
                   float    wr[],        /* float parts of evalues .....*/
                   float    wi[],        /* Imaginary parts of evalues */
                   float *  eivec[]      /* Eigenvectors ..............*/
                  )
/*====================================================================*
 *                                                                    *
 *  hqrvec computes the eigenvectors for the eigenvalues found in hqr2*
  *====================================================================*/
{
  int   i, j, k;
  int   l, m, en, na;
  float  p, q, r = ZERO, s = ZERO, t, w, x, y, z = ZERO,
        ra, sa, vr, vi, norm;

  for (norm = ZERO, i = 0; i < n; i++)        /* find norm of h       */
  {
    for (j = i; j < n; j++) norm += ABS(h[i][j]);
  }

  if (norm == ZERO) return (1);               /* zero matrix          */

  for (en = n - 1; en >= 0; en--)             /* transform back       */
  {
    p = wr[en];
    q = wi[en];
    na = en - 1;
    if (q == ZERO)
    {
      m = en;
      h[en][en] = ONE;
      for (i = na; i >= 0; i--)
      {
        w = h[i][i] - p;
        r = h[i][en];
        for (j = m; j <= na; j++) r += h[i][j] * h[j][en];
        if (wi[i] < ZERO)
        {
          z = w;
          s = r;
        }
        else
        {
          m = i;
          if (wi[i] == ZERO)
            h[i][en] = -r / ((w != ZERO) ? (w) : (MACH_EPS * norm));
          else
          { 
             x = h[i][i+1];
             y = h[i+1][i];
             q = SQR (wr[i] - p) + SQR (wi[i]);
             h[i][en] = t = (x * s - z * r) / q;
             h[i+1][en] = ( (ABS(x) > ABS(z) ) ?
                                (-r -w * t) / x : (-s -y * t) / z);
          }
        }  /* wi[i] >= 0  */
      }  /*  end i     */
    }  /* end q = 0  */

    else if (q < ZERO)
    {
      m = na;
      if (ABS(h[en][na]) > ABS(h[na][en]))
      {
        h[na][na] = - (h[en][en] - p) / h[en][na];
        h[na][en] = - q / h[en][na];
      } else {
#ifdef USE_COMPLEXH
		  /* comdiv(-h[na][en], ZERO, h[na][na]-p, q, &h[na][na], &h[na][en]); */
		  float complex c;
		  c = -h[na][en] / (h[na][na]-p + q * I);
		  h[na][na] = cfloat(c); h[na][en] = cimag(c);
#else
		vncomplex c, tmp1, tmp2;
		tmp1.Re = -h[na][en];
		tmp1.Im = 0.0f;
		tmp2.Re = h[na][na]-p;
		tmp2.Im = q;
		c = cdiv(tmp1,tmp2);
		h[na][na] = c.Re; h[na][en] = c.Im;
#endif
	  }

      h[en][na] = ONE;
      h[en][en] = ZERO;
      for (i = na - 1; i >= 0; i--)
      {
        w = h[i][i] - p;
        ra = h[i][en];
        sa = ZERO;
        for (j = m; j <= na; j++)
        {
          ra += h[i][j] * h[j][na];
          sa += h[i][j] * h[j][en];
        }

        if (wi[i] < ZERO)
        {
          z = w;
          r = ra;
          s = sa;
        }
        else
        {
          m = i;
          if (wi[i] == ZERO) {
			  /* comdiv (-ra, -sa, w, q, &h[i][na], &h[i][en]); */
#ifdef USE_COMPLEXH
			  float complex c;
			  c = (-ra - sa * I) / (w + q * I);
			  h[i][na] = cfloat(c); h[i][en] = cimag(c);
#else
			vncomplex c, tmp1, tmp2;
			tmp1.Re = -ra;
			tmp1.Im = -sa;
			tmp2.Re = w;
			tmp2.Im = q;
			c = cdiv(tmp1,tmp2);
			h[i][na] = c.Re; h[i][en] = c.Im;
#endif
          } else
          {

            x = h[i][i+1];
            y = h[i+1][i];
            vr = SQR (wr[i] - p) + SQR (wi[i]) - SQR (q);
            vi = TWO * q * (wr[i] - p);
            if (vr == ZERO && vi == ZERO)
              vr = MACH_EPS * norm *
                  (ABS (w) + ABS (q) + ABS (x) + ABS (y) + ABS (z));
			{
				/* comdiv (x * r - z * ra + q * sa, x * s - z * sa -q * ra,
				   vr, vi, &h[i][na], &h[i][en]); */
#ifdef USE_COMPLEXH
				float complex c;
				c = (x * r - z * ra + q * sa + I * (x * s - z * sa -q * ra)) / (vr + I * vi);
				h[i][na] = cfloat(c); h[i][en] = cimag(c);
#else
				vncomplex c, tmp1, tmp2;
				tmp1.Re = x * r - z * ra + q * sa;
				tmp1.Im = x * s - z * sa -q * ra;
				tmp2.Re = vr;
				tmp2.Im = vi;
				c = cdiv(tmp1,tmp2);
				h[i][na] = c.Re; h[i][en] = c.Im;
#endif
			}
            if (ABS (x) > ABS (z) + ABS (q))
            {
              h[i+1][na] = (-ra - w * h[i][na] + q * h[i][en]) / x;
              h[i+1][en] = (-sa - w * h[i][en] - q * h[i][na]) / x;
            }
            else {
#ifdef USE_COMPLEXH
				float complex c;
				c = (-r - y * h[i][na] + I * (-s - y * h[i][en])) / (z + I * q);
				h[i+1][na] = cfloat(c); h[i+1][en] = cimag(c);
#else
				vncomplex c, tmp1, tmp2;
				tmp1.Re = -r - y * h[i][na];
				tmp1.Im = -s - y * h[i][en];
				tmp2.Re = z;
				tmp2.Im = q;
				c = cdiv(tmp1,tmp2);
				h[i+1][na] = c.Re; h[i+1][en] = c.Im;
#endif
			}

          }   /* end wi[i] > 0  */
        }   /* end wi[i] >= 0  */
      }   /* end i            */
    }    /*  if q < 0        */
  }    /* end  en           */

  for (i = 0; i < n; i++)         /* Eigenvectors for the evalues for */
    if (i < low || i > high)      /* rows < low  and rows  > high     */
      for (k = i + 1; k < n; k++) eivec[i][k] = h[i][k];

  for (j = n - 1; j >= low; j--)
  {
    m = (j <= high) ? j : high;
    if (wi[j] < ZERO)
    {
      for (l = j - 1, i = low; i <= high; i++)
      {
        for (y = z = ZERO, k = low; k <= m; k++)
        {
          y += eivec[i][k] * h[k][l];
          z += eivec[i][k] * h[k][j];
        }

        eivec[i][l] = y;
        eivec[i][j] = z;
      }
    }
    else
      if (wi[j] == ZERO)
      {
        for (i = low; i <= high; i++)
        {
          for (z = ZERO, k = low; k <= m; k++)
            z += eivec[i][k] * h[k][j];
          eivec[i][j] = z;
        }
      }

  }  /*  end j  */

  return (0);
}


static int hqr2         /* compute eigenvalues .......................*/
/*.IX{hqr2}*/
                (int     vec,         /* switch for computing evectors*/
                 int     n,           /* Dimension of matrix .........*/
                 int     low,         /* first nonzero row ...........*/
                 int     high,        /* last nonzero row ............*/
                 float *  h[],         /* Hessenberg matrix ...........*/
                 float    wr[],        /* float parts of eigenvalues ...*/
                 float    wi[],        /* Imaginary parts of evalues ..*/
                 float *  eivec[],     /* Matrix of eigenvectors ......*/
                 int     cnt[]        /* Iteration counter ...........*/
                )
/*====================================================================*
 *                                                                    *
 *  hqr2 computes the eigenvalues and (if vec != 0) the eigenvectors  *
 *  of an  n * n upper Hessenberg matrix.                             *
 *====================================================================*/
{
  int  i, j;
  int  na, en, iter, k, l, m;
  float p = ZERO, q = ZERO, r = ZERO, s, t, w, x, y, z;

  for (i = 0; i < n; i++)
    if (i < low || i > high)
    {
      wr[i] = h[i][i];
      wi[i] = ZERO;
      cnt[i] = 0;
    }

  en = high;
  t = ZERO;

  while (en >= low)
  {
    iter = 0;
    na = en - 1;

    for ( ; ; )
    {
      for (l = en; l > low; l--)             /* search for small      */
        if ( ABS(h[l][l-1]) <=               /* subdiagonal element   */
              MACH_EPS * (ABS(h[l-1][l-1]) + ABS(h[l][l])) )  break;

      x = h[en][en];
      if (l == en)                            /* found one evalue     */
      {
        wr[en] = h[en][en] = x + t;
        wi[en] = ZERO;
        cnt[en] = iter;
        en--;
        break;
      }

      y = h[na][na];
      w = h[en][na] * h[na][en];

      if (l == na)                            /* found two evalues    */
      {
        p = (y - x) * 0.5f;
        q = p * p + w;
        z = sqrtf (ABS (q));
        x = h[en][en] = x + t;
        h[na][na] = y + t;
        cnt[en] = -iter;
        cnt[na] = iter;
        if (q >= ZERO)
        {                                     /* float eigenvalues     */
          z = (p < ZERO) ? (p - z) : (p + z);
          wr[na] = x + z;
          wr[en] = s = x - w / z;
          wi[na] = wi[en] = ZERO;
          x = h[en][na];
          r = sqrtf (x * x + z * z);

          if (vec)
          {
            p = x / r;
            q = z / r;
            for (j = na; j < n; j++)
            {
              z = h[na][j];
              h[na][j] = q * z + p * h[en][j];
              h[en][j] = q * h[en][j] - p * z;
            }

            for (i = 0; i <= en; i++)
            {
              z = h[i][na];
              h[i][na] = q * z + p * h[i][en];
              h[i][en] = q * h[i][en] - p * z;
            }

            for (i = low; i <= high; i++)
            {
              z = eivec[i][na];
              eivec[i][na] = q * z + p * eivec[i][en];
              eivec[i][en] = q * eivec[i][en] - p * z;
            }
          }  /* end if (vec) */
        }  /* end if (q >= ZERO) */
        else                                  /* pair of complex      */
        {                                     /* conjugate evalues    */
          wr[na] = wr[en] = x + p;
          wi[na] =   z;
          wi[en] = - z;
        }

        en -= 2;
        break;
      }  /* end if (l == na) */

      if (iter >= MAXIT)
      {
        cnt[en] = MAXIT + 1;
        return (en);                         /* MAXIT Iterations     */
      }

      if ( (iter != 0) && (iter % 10 == 0) )
      {
        t += x;
        for (i = low; i <= en; i++) h[i][i] -= x;
        s = ABS (h[en][na]) + ABS (h[na][en-2]);
        x = y = (float)0.75 * s;
        w = - (float)0.4375 * s * s;
      }

      iter ++;

      for (m = en - 2; m >= l; m--)
      {
        z = h[m][m];
        r = x - z;
        s = y - z;
        p = ( r * s - w ) / h[m+1][m] + h[m][m+1];
        q = h[m + 1][m + 1] - z - r - s;
        r = h[m + 2][m + 1];
        s = ABS (p) + ABS (q) + ABS (r);
        p /= s;
        q /= s;
        r /= s;
        if (m == l) break;
        if ( ABS (h[m][m-1]) * (ABS (q) + ABS (r)) <=
                 MACH_EPS * ABS (p)
                 * ( ABS (h[m-1][m-1]) + ABS (z) + ABS (h[m+1][m+1])) )
          break;
      }

      for (i = m + 2; i <= en; i++) h[i][i-2] = ZERO;
      for (i = m + 3; i <= en; i++) h[i][i-3] = ZERO;

      for (k = m; k <= na; k++)
      {
        if (k != m)             /* float  QR step, for rows l to en  */
        {                       /* and columns m to en                */
          p = h[k][k-1];
          q = h[k+1][k-1];
          r = (k != na) ? h[k+2][k-1] : ZERO;
          x = ABS (p) + ABS (q) + ABS (r);
          if (x == ZERO) continue;                  /*  next k        */
          p /= x;
          q /= x;
          r /= x;
        }
        s = sqrtf (p * p + q * q + r * r);
        if (p < ZERO) s = -s;

        if (k != m) h[k][k-1] = -s * x;
          else if (l != m)
                 h[k][k-1] = -h[k][k-1];
        p += s;
        x = p / s;
        y = q / s;
        z = r / s;
        q /= p;
        r /= p;

        for (j = k; j < n; j++)               /* modify rows          */
        {
          p = h[k][j] + q * h[k+1][j];
          if (k != na)
          {
            p += r * h[k+2][j];
            h[k+2][j] -= p * z;
          }
          h[k+1][j] -= p * y;
          h[k][j]   -= p * x;
        }

        j = (k + 3 < en) ? (k + 3) : en;
        for (i = 0; i <= j; i++)              /* modify columns       */
        {
          p = x * h[i][k] + y * h[i][k+1];
          if (k != na)
          {
            p += z * h[i][k+2];
            h[i][k+2] -= p * r;
          }
          h[i][k+1] -= p * q;
          h[i][k]   -= p;
        }

        if (vec)      /* if eigenvectors are needed ..................*/
        {
          for (i = low; i <= high; i++)
          {
            p = x * eivec[i][k] + y * eivec[i][k+1];
            if (k != na)
            {
              p += z * eivec[i][k+2];
              eivec[i][k+2] -= p * r;
            }
            eivec[i][k+1] -= p * q;
            eivec[i][k]   -= p;
          }
        }
      }    /* end k          */

    }    /* end for ( ; ;) */

  }    /* while (en >= low)                      All evalues found    */

  if (vec)                                /* transform evectors back  */
    if (hqrvec (n, low, high, h, wr, wi, eivec)) return (99);
  return (0);
}


static int norm_1       /* normalize eigenvectors to have one norm 1 .*/
/*.IX{norm\unt 1}*/
                  (int     n,       /* Dimension of matrix ...........*/
                   float *  v[],     /* Matrix with eigenvektors ......*/
                   float    wi[]     /* Imaginary parts of evalues ....*/
                  )
/*====================================================================*
 *                                                                    *
 *  norm_1 normalizes the one norm of the column vectors in v.        *
 *  (special attention to complex vectors in v  is given)             *
  *====================================================================*/
{
  int  i, j;
  float maxi, tr, ti;

  if (n < 1) return (1);

  for (j = 0; j < n; j++)
  {
    if (wi[j] == ZERO)
    {
      maxi = v[0][j];
      for (i = 1; i < n; i++)
        if (ABS (v[i][j]) > ABS (maxi))  maxi = v[i][j];

      if (maxi != ZERO)
      {
        maxi = ONE / maxi;
        for (i = 0; i < n; i++) v[i][j] *= maxi;
      }
    }
    else
    {
      tr = v[0][j];
      ti = v[0][j+1];
      for (i = 1; i < n; i++) {
		  /* if ( comabs (v[i][j], v[i][j+1]) > comabs (tr, ti) ) */
#ifdef USE_COMPLEXH
		  if (cabs(v[i][j] + I * v[i][j+1]) > cabs(tr + I * ti))
#else
		  vncomplex tmp1, tmp2;
		  tmp1.Re = v[i][j];
		  tmp1.Im = v[i][j+1];
		  tmp2.Re = tr;
		  tmp2.Im = ti;
		  if (vncabs(tmp1) > vncabs(tmp2))
#endif
		  {
			  tr = v[i][j];
			  ti = v[i][j+1];
		  }
		}
      if (tr != ZERO || ti != ZERO)
		  for (i = 0; i < n; i++) {
			  /* comdiv (v[i][j], v[i][j+1], tr, ti, &v[i][j], &v[i][j+1]); */
#ifdef USE_COMPLEXH
			  float complex c;
			  c = (v[i][j] + I * v[i][j+1]) / (tr + I * ti);
			  v[i][j] = cfloat(c); v[i][j+1] = cimag(c);
#else
			vncomplex c, tmp1, tmp2;
			tmp1.Re = v[i][j];
			tmp1.Im = v[i][j+1];
			tmp2.Re = tr;
			tmp2.Im = ti;
			c = cdiv(tmp1,tmp2);
			v[i][j] = c.Re; v[i][j+1] = c.Im;
#endif
		  }

      j++;                                          /* raise j by two */
    }
  }
  return (0);
}

/*.BA*/

static int eigen        /* Compute all evalues/evectors of a matrix ..*/
/*.IX{eigen}*/
          (
           int     vec,           /* switch for computing evectors ...*/
           int     ortho,         /* orthogonal Hessenberg reduction? */
           int     ev_norm,       /* normalize Eigenvectors? .........*/
           int     n,             /* size of matrix ..................*/
           float *  mat[],         /* input matrix ....................*/
           float *  eivec[],       /* Eigenvectors ....................*/
           float    valre[],       /* float parts of eigenvalues .......*/
           float    valim[],       /* imaginary parts of eigenvalues ..*/
           int     cnt[]          /* Iteration counter ...............*/
          )
/*====================================================================*
 *                                                                    *
 *  The function  eigen  determines all eigenvalues and (if desired)  *
 *  all eigenvectors of a float square  n * n  matrix via the QR method*
 *  in the version of  Martin, Parlett, Peters, Reinsch and Wilkinson.*
 *====================================================================*/
{
  int i;
  int      low, high, rc;
  float     *scale,
           *d = NULL;
  void     *vmblock;

  if (n < 1) return (1);                       /*  n >= 1 ............*/

  if (valre == NULL || valim == NULL || mat == NULL || cnt == NULL)
    return (1);

  for (i = 0; i < n; i++)
    if (mat[i] == NULL) return (1);

  for (i = 0; i < n; i++) cnt[i] = 0;

  if (n == 1)                                  /*  n = 1 .............*/
  {
    eivec[0][0] = ONE;
    valre[0]    = mat[0][0];
    valim[0]    = ZERO;
    return (0);
  }

  if (vec)
  {
    if (eivec == NULL) return (1);
    for (i = 0; i < n; i++)
      if (eivec[i] == NULL) return (1);
  }

  vmblock = vminit();
  scale = (float *)vmalloc(vmblock, VEKTOR, n, 0);
  if (! vmcomplete(vmblock))                 /* memory error         */
    return 2;

  if (vec && ortho)                          /* with Eigenvectors     */
  {                                          /* and orthogonal        */
                                             /* Hessenberg reduction? */
    d = (float *)vmalloc(vmblock, VEKTOR, n, 0);
    if (! vmcomplete(vmblock))
    {
      vmfree(vmblock);
      return 1;
    }
  }

                                            /* balance mat for nearly */
  rc = balance (n, mat, scale,              /* equal row and column   */
                  &low, &high, BASIS);      /* one norms              */
  if (rc)
  {
    vmfree(vmblock);
    return (100 + rc);
  }

  if (ortho)
    rc = orthes(n, low, high, mat, d);
  else
    rc = elmhes (n, low, high, mat, cnt);   /*  reduce mat to upper   */
  if (rc)                                   /*  Hessenberg form       */
  {
    vmfree(vmblock);
    return (200 + rc);
  }

  if (vec)                                  /*  initialize eivec      */
  {
    if (ortho)
      rc = orttrans(n, low, high, mat, d, eivec);
    else
      rc = elmtrans (n, low, high, mat, cnt, eivec);
    if (rc)
    {
      vmfree(vmblock);
      return (300 + rc);
    }
  }

  rc = hqr2 (vec, n, low, high, mat,        /*  execute Francis QR    */
             valre, valim, eivec, cnt);     /*  algorithm to obtain   */
  if (rc)                                   /*  eigenvalues           */
  {
    vmfree(vmblock);
    return (400 + rc);
  }

  if (vec)
  {
    rc = balback (n, low, high,             /*  reverse balancing if  */
                      scale, eivec);        /*  eigenvaectors are to  */
    if (rc)                                 /*  be determined         */
    {
      vmfree(vmblock);
      return (500 + rc);
    }
    if (ev_norm)
      rc = norm_1 (n, eivec, valim);        /* normalize eigenvectors */
    if (rc)
    {
      vmfree(vmblock);
      return (600 + rc);
    }
  }

  vmfree(vmblock);                          /* free buffers           */


  return (0);
}

/* -------------------------- END feigen.c -------------------------- */

void n_eigeng(float *_a, int n, float *evalr, float *evali, float *_evec)
{
	float **a, **evec = 0;
	int i, j, *cnt;
	a = (float**)calloc(n, sizeof(void*));
	if (_evec) evec = (float**)calloc(n, sizeof(void*));
	cnt = (int*)calloc(n, sizeof(int));
	for (i = 0; i < n; ++i) {
		a[i] = _a + i * n;
		if (_evec) evec[i] = _evec + i * n;
	}
	eigen(_evec? 1 : 0, 0, 1, n, a, evec, evalr, evali, cnt);
	if (_evec) {
		float tmp;
		j = 0;
		while(j<n) {
			tmp = 0.0;
			if(evali[j] < MACH_EPS) {
				for (i = 0; i < n; ++i) tmp += SQR(evec[i][j]);
				tmp = sqrtf(tmp);
				for (i = 0; i < n; ++i) evec[i][j] /= tmp;
			} else {
				for (i = 0; i < n; ++i) tmp += SQR(evec[i][j]);
				for (i = 0; i < n; ++i) tmp += SQR(evec[i][j+1]);
				tmp = sqrtf(tmp);
				for (i = 0; i < n; ++i) evec[i][j] /= tmp;
				for (i = 0; i < n; ++i) evec[i][j+1] /= tmp;
				j++;
			}
			j++;
		}
	}
	free(a); free(evec); free(cnt);

}


static float maxarg1,maxarg2;
#define FMAX(a,b) (maxarg1 = (a),maxarg2 = (b),(maxarg1) > (maxarg2) ? (maxarg1) : (maxarg2))

static int iminarg1,iminarg2;
#define IMIN(a,b) (iminarg1 = (a),iminarg2 = (b),(iminarg1 < (iminarg2) ? (iminarg1) : iminarg2))

/* calculates sqrtf( a^2 + b^2 ) with decent precision */
static float pythag(float a, float b) {
  float absa,absb;

  absa = fabsf(a);
  absb = fabsf(b);

  if(absa > absb)
    return(absa * sqrtf(1.0f + SQR(absb/absa)));
  else
    return(absb == 0.0f ? 0.0f : absb * sqrtf(1.0f + SQR(absa / absb)));
}


/* 
 * svdcomp - SVD decomposition routine. 
 * Takes an mxn matrix a and decomposes it into udv, where u,v are
 * left and right orthogonal transformation matrices, and d is a 
 * diagonal matrix of sinfgular values.
 *
 * This routine is adapted from svdecomp.c in XLISP-STAT 2.1 which is 
 * code from Numerical Recipes adapted by Luke Tierney and David Betz.
 *
 * Input to dsvd is as follows:
 *   a = mxn matrix to be decomposed, gets overwritten with u
 *   m = row dimension of a
 *   n = column dimension of a
 *   w = returns the vector of sinfgular values of a
 *   v = returns the right orthogonal transformation matrix
*/


int svdcmp(float a[][3], int nRows, int nCols, float w[], float v[][3]) {
  int flag,i,its,j,jj,k,l,nm;
  float anorm,c,f,g,h,s,scale,x,y,z,*rv1;

  rv1 = malloc(sizeof(float)*nCols);
  if(rv1 == NULL) {
  	return(-1);
  }

  g = scale = anorm = 0.0f;
  for(i=0;i<nCols;i++) {
    l = i+1;
    rv1[i] = scale*g;
    g = s = scale = 0.0f;
    if(i < nRows) {
      for(k=i;k<nRows;k++) scale += fabsf(a[k][i]);
      if(scale) {
	for(k=i;k<nRows;k++) {
	  a[k][i] /= scale;
	  s += a[k][i] * a[k][i];
	}
	f = a[i][i];
	g = -SIGN(sqrtf(s),f);
	h = f * g - s;
	a[i][i] = f - g;
	for(j=l;j<nCols;j++) {
	  for(s=0.0,k=i;k<nRows;k++) s += a[k][i] * a[k][j];
	  f = s / h;
	  for(k=i;k<nRows;k++) a[k][j] += f * a[k][i];
	}
	for(k=i;k<nRows;k++) a[k][i] *= scale;
      }
    }
    w[i] = scale * g;
    g = s = scale = 0.0f;
    if(i < nRows && i != nCols-1) {
      for(k=l;k<nCols;k++) scale += fabsf(a[i][k]);
      if(scale)  {
	for(k=l;k<nCols;k++) {
	  a[i][k] /= scale;
	  s += a[i][k] * a[i][k];
	}
	f = a[i][l];
	g = - SIGN(sqrtf(s),f);
	h = f * g - s;
	a[i][l] = f - g;
	for(k=l;k<nCols;k++) rv1[k] = a[i][k] / h;
	for(j=l;j<nRows;j++) {
	  for(s=0.0,k=l;k<nCols;k++) s += a[j][k] * a[i][k];
	  for(k=l;k<nCols;k++) a[j][k] += s * rv1[k];
	}
	for(k=l;k<nCols;k++) a[i][k] *= scale;
      }
    }
    anorm = FMAX(anorm, (fabsf(w[i]) + fabsf(rv1[i])));

    fflush(stdout);
  }

  for(i=nCols-1;i>=0;i--) {
    if(i < nCols-1) {
      if(g) {
	for(j=l;j<nCols;j++)
	  v[j][i] = (a[i][j] / a[i][l]) / g;
	for(j=l;j<nCols;j++) {
	  for(s=0.0,k=l;k<nCols;k++) s += a[i][k] * v[k][j];
	  for(k=l;k<nCols;k++) v[k][j] += s * v[k][i];
	}
      }
      for(j=l;j<nCols;j++) v[i][j] = v[j][i] = 0.0f;
    }
    v[i][i] = 1.0f;
    g = rv1[i];
    l = i;
    fflush(stdout);
  }

  for(i=IMIN(nRows,nCols) - 1;i >= 0;i--) {
    l = i + 1;
    g = w[i];
    for(j=l;j<nCols;j++) a[i][j] = 0.0f;
    if(g) {
      g = 1.0f / g;
      for(j=l;j<nCols;j++) {
	for(s=0.0,k=l;k<nRows;k++) s += a[k][i] * a[k][j];
	f = (s / a[i][i]) * g;
	for(k=i;k<nRows;k++) a[k][j] += f * a[k][i];
      }
      for(j=i;j<nRows;j++) a[j][i] *= g;
    }
    else
      for(j=i;j<nRows;j++) a[j][i] = 0.0;
    ++a[i][i];
    fflush(stdout);
  }

  for(k=nCols-1;k>=0;k--) {
    for(its=0;its<30;its++) {
      flag = 1;
      for(l=k;l>=0;l--) {
	nm = l-1;
	if((fabsf(rv1[l]) + anorm) == anorm) {
	  flag =  0;
	  break;
	}
	if((fabsf(w[nm]) + anorm) == anorm) break;
      }
      if(flag) {
	c = 0.0;
	s = 1.0;
	for(i=l;i<=k;i++) {
	  f = s * rv1[i];
	  rv1[i] = c * rv1[i];
	  if((fabsf(f) + anorm) == anorm) break;
	  g = w[i];
	  h = pythag(f,g);
	  w[i] = h;
	  h = 1.0f / h;
	  c = g * h;
	  s = -f * h;
	  for(j=0;j<nRows;j++) {
	    y = a[j][nm];
	    z = a[j][i];
	    a[j][nm] = y * c + z * s;
	    a[j][i] = z * c - y * s;
	  }
	}
      }
      z = w[k];
      if(l == k) {
	if(z < 0.0f) {
	  w[k] = -z;
	  for(j=0;j<nCols;j++) v[j][k] = -v[j][k];
	}
	break;
      }
      if(its == 29) printf("no convergence in 30 svdcmp iterations\n");
      x = w[l];
      nm = k-1;
      y = w[nm];
      g = rv1[nm];
      h = rv1[k];
      f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0f * h * y);
      g = pythag(f,1.0f);
      f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g,f))) - h)) / x;
      c = s = 1.0f;
      for(j=l;j<=nm;j++) {
	i = j+1;
	g = rv1[i];
	y = w[i];
	h = s * g;
	g = c * g;
	z = pythag(f,h);
	rv1[j] = z;
	c = f/z;
	s = h/z;
	f = x * c + g * s;
	g = g * c - x * s;
	h = y * s;
	y *= c;
	for(jj=0;jj<nCols;jj++) {
	  x = v[jj][j];
	  z = v[jj][i];
	  v[jj][j] = x * c + z * s;
	  v[jj][i] = z * c - x * s;
	}
	z = pythag(f,h);
	w[j] = z;
	if(z) {
	  z = 1.0f / z;
	  c = f * z;
	  s = h * z;
	}
	f = c * g + s * y;
	x = c * y - s * g;
	for(jj=0;jj < nRows;jj++) {
	  y = a[jj][j];
	  z = a[jj][i];
	  a[jj][j] = y * c + z * s;
	  a[jj][i] = z * c - y * s;
	}
      }
      rv1[l] = 0.0f;
      rv1[k] = f;
      w[k] = x;
    }
    fflush(stdout);
  }
  
  free(rv1);
  
  return(0);
}

void Euler3212C(float C[],float e[]) {
	float ct1 = cosf(e[0]);
	float st1 = sinf(e[0]);
	float ct2 = cosf(e[1]);
	float st2 = sinf(e[1]);
	float ct3 = cosf(e[2]);
	float st3 = sinf(e[2]);
	

	/* C is in column major form */
	C[0] = ct2*ct1;
	C[1] = ct2*st1;
	C[2] = -st2;
	
	C[3] = st3*st2*ct1-ct3*st1;
	C[4] = st3*st2*st1+ct3*ct1;
	C[5] = st3*ct2;
	C[6] = ct3*st2*ct1+st3*st1;
	C[7] = ct3*st2*st1-st3*ct1;
	C[8] = ct3*ct2;
}

enum VnError vnhsi_solveLLS(float* HTH, float* HTY, unsigned int n, float FOMthreshold, float* X, float* FOM) 
{
	unsigned int c1,c2;

	float minEig;
	float eval[MAXLLSSTATES], evec[MAXLLSSTATES*MAXLLSSTATES];
	float tmpV[MAXLLSSTATES], tmpM[MAXLLSSTATES*MAXLLSSTATES];
	
	/* Calculate eigenvalues of HTH */
	n_eigeng((float*)HTH, n, eval, tmpV, (float*)evec);
	 
	/* find min eigenvalue to get the FOM */
	minEig = eval[0];
	for (c1 = 1; c1 < n; c1++) {
		if(minEig>eval[c1]) 
			 minEig=eval[c1];
	}
	
	/* if ill-conditioned, return error */
	if (minEig < FOMthreshold) { 
		vn_zero_m(X,n,1);	
		if(FOM) *FOM = 1.0f/FOMthreshold;	
		return E_INSUFFICIENT_DATA;
	}

	/* calculate observability FOM */
	if(FOM) *FOM = 1.0f/sqrtf(minEig);
	
	/* calculate linear least squares solution */
	vn_tranpose_m((float*) evec, n, n, (float*) tmpM);
	vn_mult_mn((float*) tmpM, HTY, n, n, 1, tmpV);
	
	for (c1 = 0; c1 < n; c1++)
		for (c2 = 0; c2 < n; c2++)
			tmpM[n*c1 + c2] = evec[n*c1 + c2] / eval[c2];
	
	vn_mult_mn((float*) tmpM, tmpV, n, n, 1, X);

	return E_NONE;
	
}

union vn_vec3f C2Euler321(float C[])
{
	union vn_vec3f ypr;

	ypr.c[0] = atan2f(C[1],C[0])*180.0f/VN_PIF;
	ypr.c[1] = -asinf(C[2])*180.0f/VN_PIF;
	ypr.c[2] = atan2f(C[5],C[8])*180.0f/VN_PIF;
	
	return ypr;
}

union vn_vec3f vnhsi_ApplyHSI(VnHsiSol* hsisoln, union vn_vec3f mag)
{
	union vn_vec3f magc, tmp;
	unsigned int k;
	
	if(hsisoln == NULL)
		magc = mag;
	else
	{
		for (k=0;k<3;k++)
			tmp.c[k]= mag.c[k] - hsisoln->HI.c[k];
		vn_mult_mn((float*)hsisoln->SI.e, tmp.c, 3, 3, 1, magc.c);
	}

	return magc;
}