/**
 *
 * @file core_ztstrf.c
 *
 *  PLASMA core_blas kernel
 *  PLASMA is a software package provided by Univ. of Tennessee,
 *  Univ. of California Berkeley and Univ. of Colorado Denver
 *
 * @version 2.6.0
 * @author Hatem Ltaief
 * @author Mathieu Faverge
 * @author Jakub Kurzak
 * @date 2010-11-15
 * @precisions normal z -> c d s
 *
 **/
#include "common.h"
#include <cblas.h>
#include <math.h>

/***************************************************************************//**
 *
 * @ingroup CORE_PLASMA_Complex64_t
 *
 *  CORE_ztstrf computes an LU factorization of a complex matrix formed
 *  by an upper triangular NB-by-N tile U on top of a M-by-N tile A
 *  using partial pivoting with row interchanges.
 *
 *  This is the right-looking Level 2.5 BLAS version of the algorithm.
 *
 *******************************************************************************
 *
 * @param[in] M
 *         The number of rows of the tile A.  M >= 0.
 *
 * @param[in] N
 *         The number of columns of the tile A.  N >= 0.
 *
 * @param[in] IB
 *         The inner-blocking size.  IB >= 0.
 *
 * @param[in] NB
 *
 * @param[in,out] U
 *         On entry, the NB-by-N upper triangular tile.
 *         On exit, the new factor U from the factorization
 *
 * @param[in] LDU
 *         The leading dimension of the array U.  LDU >= max(1,NB).
 *
 * @param[in,out] A
 *         On entry, the M-by-N tile to be factored.
 *         On exit, the factor L from the factorization
 *
 * @param[in] LDA
 *         The leading dimension of the array A.  LDA >= max(1,M).
 *
 * @param[in,out] L
 *         On entry, the IB-by-N lower triangular tile.
 *         On exit, the interchanged rows form the tile A in case of pivoting.
 *
 * @param[in] LDL
 *         The leading dimension of the array L.  LDL >= max(1,IB).
 *
 * @param[out] IPIV
 *         The pivot indices; for 1 <= i <= min(M,N), row i of the
 *         tile U was interchanged with row IPIV(i) of the tile A.
 *
 * @param[in,out] WORK
 *
 * @param[in] LDWORK
 *         The leading dimension of the array WORK.
 *
 * @param[out] INFO
 *
 *******************************************************************************
 *
 * @return
 *         \retval PLASMA_SUCCESS successful exit
 *         \retval <0 if INFO = -k, the k-th argument had an illegal value
 *         \retval >0 if INFO = k, U(k,k) is exactly zero. The factorization
 *              has been completed, but the factor U is exactly
 *              singular, and division by zero will occur if it is used
 *              to solve a system of equations.
 *
 ******************************************************************************/

#if defined(PLASMA_HAVE_WEAK)
#pragma weak CORE_ztstrf = PCORE_ztstrf
#define CORE_ztstrf PCORE_ztstrf
#define CORE_zssssm PCORE_zssssm
int  CORE_zssssm(int M1, int N1, int M2, int N2, int K, int IB,
                 PLASMA_Complex64_t *A1, int LDA1,
                 PLASMA_Complex64_t *A2, int LDA2,
                 const PLASMA_Complex64_t *L1, int LDL1,
                 const PLASMA_Complex64_t *L2, int LDL2,
                 const int *IPIV);
#endif
int CORE_ztstrf(int M, int N, int IB, int NB,
                PLASMA_Complex64_t *U, int LDU,
                PLASMA_Complex64_t *A, int LDA,
                PLASMA_Complex64_t *L, int LDL,
                int *IPIV,
                PLASMA_Complex64_t *WORK, int LDWORK,
                int *INFO)
{
    static PLASMA_Complex64_t zzero = 0.0;
    static PLASMA_Complex64_t mzone =-1.0;

    PLASMA_Complex64_t alpha;
    int i, j, ii, sb;
    int im, ip;

    /* Check input arguments */
    *INFO = 0;
    if (M < 0) {
        coreblas_error(1, "Illegal value of M");
        return -1;
    }
    if (N < 0) {
        coreblas_error(2, "Illegal value of N");
        return -2;
    }
    if (IB < 0) {
        coreblas_error(3, "Illegal value of IB");
        return -3;
    }
    if ((LDU < max(1,NB)) && (NB > 0)) {
        coreblas_error(6, "Illegal value of LDU");
        return -6;
    }
    if ((LDA < max(1,M)) && (M > 0)) {
        coreblas_error(8, "Illegal value of LDA");
        return -8;
    }
    if ((LDL < max(1,IB)) && (IB > 0)) {
        coreblas_error(10, "Illegal value of LDL");
        return -10;
    }

    /* Quick return */
    if ((M == 0) || (N == 0) || (IB == 0))
        return PLASMA_SUCCESS;

    /* Set L to 0 */
    memset(L, 0, LDL*N*sizeof(PLASMA_Complex64_t));

    ip = 0;
    for (ii = 0; ii < N; ii += IB) {
        sb = min(N-ii, IB);

        for (i = 0; i < sb; i++) {
            im = cblas_izamax(M, &A[LDA*(ii+i)], 1);
            IPIV[ip] = ii+i+1;

            if (cabs(A[LDA*(ii+i)+im]) > cabs(U[LDU*(ii+i)+ii+i])) {
                /*
                 * Swap behind.
                 */
                cblas_zswap(i, &L[LDL*ii+i], LDL, &WORK[im], LDWORK );
                /*
                 * Swap ahead.
                 */
                cblas_zswap(sb-i, &U[LDU*(ii+i)+ii+i], LDU, &A[LDA*(ii+i)+im], LDA );
                /*
                 * Set IPIV.
                 */
                IPIV[ip] = NB + im + 1;

                for (j = 0; j < i; j++) {
                    A[LDA*(ii+j)+im] = zzero;
                }
            }

            if ((*INFO == 0) && (cabs(A[LDA*(ii+i)+im]) == zzero)
                && (cabs(U[LDU*(ii+i)+ii+i]) == zzero)) {
                *INFO = ii+i+1;
            }

            alpha = ((PLASMA_Complex64_t)1. / U[LDU*(ii+i)+ii+i]);
            cblas_zscal(M, CBLAS_SADDR(alpha), &A[LDA*(ii+i)], 1);
            cblas_zcopy(M, &A[LDA*(ii+i)], 1, &WORK[LDWORK*i], 1);
            cblas_zgeru(
                CblasColMajor, M, sb-i-1,
                CBLAS_SADDR(mzone), &A[LDA*(ii+i)], 1,
                &U[LDU*(ii+i+1)+ii+i], LDU,
                &A[LDA*(ii+i+1)], LDA );
            ip = ip+1;
        }
        /*
         * Apply the subpanel to the rest of the panel.
         */
        if(ii+i < N) {
            for(j = ii; j < ii+sb; j++) {
                if (IPIV[j] <= NB) {
                    IPIV[j] = IPIV[j] - ii;
                }
            }

            CORE_zssssm(
                NB, N-(ii+sb), M, N-(ii+sb), sb, sb,
                &U[LDU*(ii+sb)+ii], LDU,
                &A[LDA*(ii+sb)], LDA,
                &L[LDL*ii], LDL,
                WORK, LDWORK, &IPIV[ii]);

            for(j = ii; j < ii+sb; j++) {
                if (IPIV[j] <= NB) {
                    IPIV[j] = IPIV[j] + ii;
                }
            }
        }
    }
    return PLASMA_SUCCESS;
}


/*
 * Copyright (c) 1997-1999 Massachusetts Institute of Technology
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program 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 General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

/* This file was automatically generated --- DO NOT EDIT */
/* Generated on Sun Nov  7 20:44:21 EST 1999 */

#include <fftw-int.h>
#include <fftw.h>

/* Generated by: ./genfft -magic-alignment-check -magic-twiddle-load-all -magic-variables 4 -magic-loopi -hc2real 10 */

/*
 * This function contains 34 FP additions, 14 FP multiplications,
 * (or, 26 additions, 6 multiplications, 8 fused multiply/add),
 * 20 stack variables, and 20 memory accesses
 */
static const fftw_real K500000000 = FFTW_KONST(+0.500000000000000000000000000000000000000000000);
static const fftw_real K1_902113032 = FFTW_KONST(+1.902113032590307144232878666758764286811397268);
static const fftw_real K1_175570504 = FFTW_KONST(+1.175570504584946258337411909278145537195304875);
static const fftw_real K2_000000000 = FFTW_KONST(+2.000000000000000000000000000000000000000000000);
static const fftw_real K1_118033988 = FFTW_KONST(+1.118033988749894848204586834365638117720309180);

/*
 * Generator Id's :
 * $Id: exprdag.ml,v 1.41 1999/05/26 15:44:14 fftw Exp $
 * $Id: fft.ml,v 1.43 1999/05/17 19:44:18 fftw Exp $
 * $Id: to_c.ml,v 1.25 1999/10/26 21:41:32 stevenj Exp $
 */

void fftw_hc2real_10(const fftw_real *real_input, const fftw_real *imag_input, fftw_real *output, int real_istride, int imag_istride, int ostride)
{
     fftw_real tmp3;
     fftw_real tmp11;
     fftw_real tmp23;
     fftw_real tmp31;
     fftw_real tmp20;
     fftw_real tmp30;
     fftw_real tmp10;
     fftw_real tmp28;
     fftw_real tmp14;
     fftw_real tmp16;
     fftw_real tmp18;
     fftw_real tmp19;
     ASSERT_ALIGNED_DOUBLE;
     {
	  fftw_real tmp1;
	  fftw_real tmp2;
	  fftw_real tmp21;
	  fftw_real tmp22;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp1 = real_input[0];
	  tmp2 = real_input[5 * real_istride];
	  tmp3 = tmp1 - tmp2;
	  tmp11 = tmp1 + tmp2;
	  tmp21 = imag_input[4 * imag_istride];
	  tmp22 = imag_input[imag_istride];
	  tmp23 = tmp21 - tmp22;
	  tmp31 = tmp21 + tmp22;
     }
     tmp18 = imag_input[2 * imag_istride];
     tmp19 = imag_input[3 * imag_istride];
     tmp20 = tmp18 - tmp19;
     tmp30 = tmp18 + tmp19;
     {
	  fftw_real tmp6;
	  fftw_real tmp12;
	  fftw_real tmp9;
	  fftw_real tmp13;
	  ASSERT_ALIGNED_DOUBLE;
	  {
	       fftw_real tmp4;
	       fftw_real tmp5;
	       fftw_real tmp7;
	       fftw_real tmp8;
	       ASSERT_ALIGNED_DOUBLE;
	       tmp4 = real_input[2 * real_istride];
	       tmp5 = real_input[3 * real_istride];
	       tmp6 = tmp4 - tmp5;
	       tmp12 = tmp4 + tmp5;
	       tmp7 = real_input[4 * real_istride];
	       tmp8 = real_input[real_istride];
	       tmp9 = tmp7 - tmp8;
	       tmp13 = tmp7 + tmp8;
	  }
	  tmp10 = tmp6 + tmp9;
	  tmp28 = K1_118033988 * (tmp6 - tmp9);
	  tmp14 = tmp12 + tmp13;
	  tmp16 = K1_118033988 * (tmp12 - tmp13);
     }
     output[5 * ostride] = tmp3 + (K2_000000000 * tmp10);
     {
	  fftw_real tmp32;
	  fftw_real tmp34;
	  fftw_real tmp29;
	  fftw_real tmp33;
	  fftw_real tmp27;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp32 = (K1_175570504 * tmp30) - (K1_902113032 * tmp31);
	  tmp34 = (K1_902113032 * tmp30) + (K1_175570504 * tmp31);
	  tmp27 = tmp3 - (K500000000 * tmp10);
	  tmp29 = tmp27 - tmp28;
	  tmp33 = tmp28 + tmp27;
	  output[7 * ostride] = tmp29 - tmp32;
	  output[3 * ostride] = tmp29 + tmp32;
	  output[ostride] = tmp33 - tmp34;
	  output[9 * ostride] = tmp33 + tmp34;
     }
     output[0] = tmp11 + (K2_000000000 * tmp14);
     {
	  fftw_real tmp24;
	  fftw_real tmp26;
	  fftw_real tmp17;
	  fftw_real tmp25;
	  fftw_real tmp15;
	  ASSERT_ALIGNED_DOUBLE;
	  tmp24 = (K1_175570504 * tmp20) - (K1_902113032 * tmp23);
	  tmp26 = (K1_902113032 * tmp20) + (K1_175570504 * tmp23);
	  tmp15 = tmp11 - (K500000000 * tmp14);
	  tmp17 = tmp15 - tmp16;
	  tmp25 = tmp16 + tmp15;
	  output[2 * ostride] = tmp17 - tmp24;
	  output[8 * ostride] = tmp17 + tmp24;
	  output[6 * ostride] = tmp25 - tmp26;
	  output[4 * ostride] = tmp25 + tmp26;
     }
}

fftw_codelet_desc fftw_hc2real_10_desc =
{
     "fftw_hc2real_10",
     (void (*)()) fftw_hc2real_10,
     10,
     FFTW_BACKWARD,
     FFTW_HC2REAL,
     235,
     0,
     (const int *) 0,
};


/* Routines for manipulating evolutionary rate matrices.
 *
 * There is no specific object for this module.  Rate matrix
 * operations use square nxn ESL_DMATRIX data objects. (The rmx
 * module essentially subclasses the dmx module.)
 *
 * An instantaneous rate matrix is usually denoted by Q.  A
 * conditional probability matrix (for a specific t) is usually
 * denoted by P. An exchangeability matrix is denoted by E.
 * A stationary residue probability vector is denoted by pi.
 *
 * Two important relations among these:
 *
 *    Q in terms of E and pi:
 *      $Q_{ij} = E_{ij} \pi_j$ for $i \neq j$;
 *      $Q_{ii} = -\sum_{j \neq i} Q_{ij}$
 *
 *    P in terms of Q and t:
 *      $P = e^{tQ}$
 *
 * Contents:
 *   1. Setting standard rate matrix models.
 *   2. Debugging routines for validating or dumping rate matrices.
 *   3. Other routines in the exposed ratematrix API.
 *   4. Benchmark driver.
 *   5. Regression test driver.
 *   6. Unit tests.
 *   7. Test driver.
 *   8. Example.
 *   9. Copyright/license information.
 *
 *
 *
 * See also:
 *   paml   - i/o of rate matrices from/to data files in PAML format
 *
 */
#include "esl_config.h"

#include <math.h>

#include "easel.h"
#include "esl_composition.h"
#include "esl_dmatrix.h"
#include "esl_vectorops.h"
#include "esl_ratematrix.h"

/*****************************************************************
 * 1. Setting standard rate matrix models.
 *****************************************************************/

/* Function:  esl_rmx_SetWAG()
 * Incept:    SRE, Thu Mar  8 18:00:00 2007 [Janelia]
 *
 * Purpose:   Sets a $20 \times 20$ rate matrix <Q> to WAG parameters.
 *            The caller allocated <Q>.
 *
 *            If <pi> is non-<NULL>, it provides a vector of 20 amino
 *            acid stationary probabilities in Easel alphabetic order,
 *            A..Y, and the WAG stationary probabilities are set to
 *            these desired $\pi_i$. If <pi> is <NULL>, the default
 *            WAG stationary probabilities are used.
 *
 *            The WAG parameters are a maximum likelihood
 *            parameterization obtained by Whelan and Goldman
 *            \citep{WhelanGoldman01}.
 *
 * Note:      The data table was reformatted from wag.dat by the UTILITY1
 *            executable in the paml module. The wag.dat file was obtained from
 *            \url{http://www.ebi.ac.uk/goldman/WAG/wag.dat}. A copy
 *            is in formats/wag.dat.
 *
 * Args:      Q   - a 20x20 rate matrix to set, allocated by caller.
 *            pi  - desired stationary probabilities A..Y, or
 *                  NULL to use WAG defaults.
 *
 * Returns:   <eslOK> on success.
 *
 * Throws:    <eslEINVAL> if <Q> isn't a 20x20 general matrix; and
 *            the state of <Q> is undefined.
 */
int
esl_rmx_SetWAG(ESL_DMATRIX *Q, double *pi)
{
  static double wagE[190] = {
    1.027040, 0.738998, 0.030295, 1.582850, 0.021352, 6.174160, 0.210494, 0.398020, 0.046730, 0.081134,
    1.416720, 0.306674, 0.865584, 0.567717, 0.049931, 0.316954, 0.248972, 0.930676, 0.570025, 0.679371,
    0.249410, 0.193335, 0.170135, 0.039437, 0.127395, 1.059470, 0.030450, 0.138190, 0.906265, 0.074034,
    0.479855, 2.584430, 0.088836, 0.373558, 0.890432, 0.323832, 0.397915, 0.384287, 0.084805, 0.154263,
    2.115170, 0.061304, 0.499462, 3.170970, 0.257555, 0.893496, 0.390482, 0.103754, 0.315124, 1.190630,
    0.174100, 0.404141, 4.257460, 0.934276, 4.854020, 0.509848, 0.265256, 5.429420, 0.947198, 0.096162,
    1.125560, 3.956290, 0.554236, 3.012010, 0.131528, 0.198221, 1.438550, 0.109404, 0.423984, 0.682355,
    0.161444, 0.243570, 0.696198, 0.099929, 0.556896, 0.415844, 0.171329, 0.195081, 0.908598, 0.098818,
    0.616783, 5.469470, 0.099921, 0.330052, 4.294110, 0.113917, 3.894900, 0.869489, 1.545260, 1.543640,
    0.933372, 0.551571, 0.528191, 0.147304, 0.439157, 0.102711, 0.584665, 2.137150, 0.186979, 5.351420,
    0.497671, 0.683162, 0.635346, 0.679489, 3.035500, 3.370790, 1.407660, 1.071760, 0.704939, 0.545931,
    1.341820, 0.740169, 0.319440, 0.967130, 0.344739, 0.493905, 3.974230, 1.613280, 1.028870, 1.224190,
    2.121110, 0.512984, 0.374866, 0.822765, 0.171903, 0.225833, 0.473307, 1.458160, 1.386980, 0.326622,
    1.516120, 2.030060, 0.795384, 0.857928, 0.554413, 4.378020, 2.006010, 1.002140, 0.152335, 0.588731,
    0.649892, 0.187247, 0.118358, 7.821300, 0.305434, 1.800340, 2.058450, 0.196246, 0.314887, 0.301281,
    0.251849, 0.232739, 1.388230, 0.113133, 0.717070, 0.129767, 0.156557, 1.529640, 0.336983, 0.262569,
    0.212483, 0.137505, 0.665309, 0.515706, 0.071917, 0.139405, 0.215737, 1.163920, 0.523742, 0.110864,
    0.365369, 0.240735, 0.543833, 0.325711, 0.196303, 6.454280, 0.103604, 3.873440, 0.420170, 0.133264,
    0.398618, 0.428437, 1.086000, 0.216046, 0.227710, 0.381533, 0.786993, 0.291148, 0.314730, 2.485390};
  static double wagpi[20];
  int i,j,z;

  if (Q->m != 20 || Q->n != 20 || Q->type != eslGENERAL)
    ESL_EXCEPTION(eslEINVAL, "Q must be a 20x20 general matrix");
  esl_composition_WAG(wagpi);

  /* 1. Transfer the wag E lower triagonal matrix directly into Q. */
  z = 0;
  for (i = 0; i < 20; i++)
    {
      Q->mx[i][i] = 0.; /* code below depends on this zero initialization */
      for (j = 0; j < i; j++) {
	Q->mx[i][j] = wagE[z++];
	Q->mx[j][i] = Q->mx[i][j];
      }
    }

  /* 2. Set offdiagonals Q_ij = E_ij * pi_j */
  for (i = 0; i < 20; i++)
    for (j = 0; j < 20; j++)
      if (pi != NULL) Q->mx[i][j] *= pi[j];
      else            Q->mx[i][j] *= wagpi[j];

  /* 3. Set diagonal Q_ii to -\sum_{i \neq j} Q_ij */
  for (i = 0; i < 20; i++)
    Q->mx[i][i] = -1. * esl_vec_DSum(Q->mx[i], 20);

  /* 4. Renormalize matrix to units of 1 substitution/site. */
  if (pi != NULL) esl_rmx_ScaleTo(Q, pi,    1.0);
  else            esl_rmx_ScaleTo(Q, wagpi, 1.0);

  return eslOK;
}

/* Function:  esl_rmx_SetJukesCantor()
 * Incept:    SRE, Thu Mar 15 13:04:56 2007 [Janelia]
 *
 * Purpose:   Sets a 4x4 rate matrix to a Jukes-Cantor model,
 *            scaled to units of 1t = 1.0 substitutions/site.
 *
 * Note:     eigenvalues of Q are 0, -4\alpha, -4\alpha, -4\alpha
 */
int
esl_rmx_SetJukesCantor(ESL_DMATRIX *Q)
{
  int    i,j;
  double pi[4] = { 0.25, 0.25, 0.25, 0.25 };

  if (Q->m != 4 || Q->n != 4 || Q->type != eslGENERAL)
    ESL_EXCEPTION(eslEINVAL, "Q must be a 4x4 general matrix");

  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++)
      {
	if (i != j) Q->mx[i][j] = 1.0;
	else        Q->mx[i][j] = 0.0;
      }
    Q->mx[i][i] =  -1. * esl_vec_DSum(Q->mx[i], 4);
  }
  esl_rmx_ScaleTo(Q, pi, 1.0);
  return eslOK;
}

/* Function:  esl_rmx_SetKimura()
 * Incept:    SRE, Thu Mar 15 13:08:08 2007 [Janelia]
 *
 * Purpose:   Sets a 4x4 rate matrix to a Kimura 2-parameter
 *            model, given transition and transversion
 *            relative rates <alpha> and <beta>, respectively,
 *            scaled to units of 1t = 1.0 substitutions/site.
 *
 * Note:     eigenvalues of Q are 0, -4\alpha, -2(\alpha+\beta), -2(\alpha+\beta)
 */
int
esl_rmx_SetKimura(ESL_DMATRIX *Q, double alpha, double beta)
{
  int i,j;
  double pi[4] = { 0.25, 0.25, 0.25, 0.25 };

  if (Q->m != 4 || Q->n != 4 || Q->type != eslGENERAL)
    ESL_EXCEPTION(eslEINVAL, "Q must be a 4x4 general matrix");

  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++)
      {
	if (i != j) Q->mx[i][j] = ((i+j)%2)? beta : alpha; /* even=0=transition;odd=1=transversion */
	else        Q->mx[i][j] = 0.0;
      }
    Q->mx[i][i] =  -1. * esl_vec_DSum(Q->mx[i], 4);
  }
  esl_rmx_ScaleTo(Q, pi, 1.0);
  return eslOK;
}

/* Function:  esl_rmx_SetF81()
 * Incept:    SRE, Thu Mar 15 13:33:30 2007 [Janelia]
 *
 * Purpose:   Sets a 4x4 rate matrix to the F81 model (aka
 *            equal-input model) given stationary base
 *            compositions <pi>,
 *            scaled to units of 1t = 1.0 substitutions/site.
 */
int
esl_rmx_SetF81(ESL_DMATRIX *Q, double *pi)
{
  int i,j;

  if (Q->m != 4 || Q->n != 4 || Q->type != eslGENERAL)
    ESL_EXCEPTION(eslEINVAL, "Q must be a 4x4 general matrix");

  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++)
      {
	if (i != j) Q->mx[i][j] = pi[j];
	else        Q->mx[i][j] = 0.0;
      }
    Q->mx[i][i] =  -1. * esl_vec_DSum(Q->mx[i], 4);
  }
  esl_rmx_ScaleTo(Q, pi, 1.0);
  return eslOK;
}

/* Function:  esl_rmx_SetHKY()
 * Incept:    SRE, Thu Aug 12 08:26:39 2004 [St. Louis]
 *
 * Purpose:   Given stationary base composition <pi> for ACGT, and
 *            transition and transversion relative rates <alpha> and
 *            <beta> respectively, sets the matrix <Q> to be the
 *            corresponding HKY (Hasegawa/Kishino/Yano) DNA rate
 *            matrix, scaled in units of 1t= 1.0 substitutions/site
 *            \citep{Hasegawa85}.
 *
 * Args:      pi     - stationary base composition A..T
 *            alpha  - relative transition rate
 *            beta   - relative transversion rate
 *
 *
 * Returns:   <eslOK>
 *
 * Xref:
 */
int
esl_rmx_SetHKY( ESL_DMATRIX *Q, double *pi, double alpha, double beta)
{
  int i,j;

  if (Q->m != 4 || Q->n != 4 || Q->type != eslGENERAL)
    ESL_EXCEPTION(eslEINVAL, "Q must be a 4x4 general matrix");

  for (i = 0; i < 4; i++) {
    for (j = 0; j < 4; j++)
      {
	if (i != j)  Q->mx[i][j] = ((i+j)%2)? pi[j]*beta : pi[j]*alpha; /* even=0=transition;odd=1=transversion */
	else         Q->mx[i][j] = 0.;
      }
    Q->mx[i][i] =  -1. * esl_vec_DSum(Q->mx[i], 4);
  }
  esl_rmx_ScaleTo(Q, pi, 1.0);
  return eslOK;
}

/*****************************************************************
 * 2. Debugging routines for validating or dumping rate matrices.
 *****************************************************************/

/* Function:  esl_rmx_ValidateP()
 * Incept:    SRE, Sun Mar 11 10:30:50 2007 [Janelia]
 *
 * Purpose:   Validates a conditional probability matrix <P>, whose
 *            elements $P_{ij}$ represent conditional probabilities
 *            $P(j \mid i)$; for example in a first-order Markov
 *            chain, or a continuous-time Markov transition process
 *            where <P> is for a particular $t$.
 *
 *            Rows must sum to one, and each element $P_{ij}$ is a
 *            probability $0 \leq P_{ij} \leq 1$.
 *
 *            <tol> specifies the floating-point tolerance to which
 *            the row sums must equal one: <fabs(sum-1.0) <= tol>.
 *
 *            <errbuf> is an optional error message buffer. The caller
 *            may pass <NULL> or a pointer to a buffer of at least
 *            <eslERRBUFSIZE> characters.
 *
 * Args:      P      - matrix to validate
 *            tol    - floating-point tolerance (0.00001, for example)
 *            errbuf - OPTIONAL: ptr to an error buffer of at least
 *                     <eslERRBUFSIZE> characters.
 *
 * Returns:   <eslOK> on successful validation.
 *            <eslFAIL> on failure, and if a non-<NULL> <errbuf> was
 *            provided by the caller, a message describing
 *            the reason for the failure is put there.
 *
 * Throws:    (no abnormal error conditions)
 */
int
esl_rmx_ValidateP(ESL_DMATRIX *P, double tol, char *errbuf)
{
  int    i,j;
  double sum;

  if (P->type != eslGENERAL) ESL_EXCEPTION(eslEINVAL, "P must be type eslGENERAL to be validated");

  for (i = 0; i < P->n; i++)
    {
      sum = esl_vec_DSum(P->mx[i], P->m);
      if (fabs(sum-1.0) > tol) ESL_FAIL(eslFAIL, errbuf, "row %d does not sum to 1.0", i);

      for (j = 0; j < P->m; j++)
	if (P->mx[i][j] < 0.0 || P->mx[i][j] > 1.0)
	  ESL_FAIL(eslFAIL, errbuf, "element %d,%d is not a probability (%f)", i,j,P->mx[i][j]);
    }
  return eslOK;
}

/* Function:  esl_rmx_ValidateQ()
 * Incept:    SRE, Sun Mar 11 10:30:50 2007 [Janelia]
 *
 * Purpose:   Validates an instantaneous rate matrix <Q> for a
 *            continuous-time Markov process, whose elements $q_{ij}$
 *            represent instantaneous transition rates $i \rightarrow
 *            j$.
 *
 *            Rows satisfy the condition that
 *            $q_{ii} = -\sum_{i \neq j} q_{ij}$, and also
 *            that $q_{ij} \geq 0$ for all $j \neq i$.
 *
 *            <tol> specifies the floating-point tolerance to which
 *            that condition must hold: <fabs(sum-q_ii) <= tol>.
 *
 *            <errbuf> is an optional error message buffer. The caller
 *            may pass <NULL> or a pointer to a buffer of at least
 *            <eslERRBUFSIZE> characters.
 *
 * Args:      Q      - rate matrix to validate
 *            tol    - floating-point tolerance (0.00001, for example)
 *            errbuf - OPTIONAL: ptr to an error buffer of at least
 *                     <eslERRBUFSIZE> characters.
 *
 * Returns:   <eslOK> on successful validation.
 *            <eslFAIL> on failure, and if a non-<NULL> <errbuf> was
 *            provided by the caller, a message describing
 *            the reason for the failure is put there.
 *
 * Throws:    (no abnormal error conditions)
 */
int
esl_rmx_ValidateQ(ESL_DMATRIX *Q, double tol, char *errbuf)
{
  int    i,j;
  double qi;

  if (Q->type != eslGENERAL) ESL_EXCEPTION(eslEINVAL, "Q must be type eslGENERAL to be validated");
  if (Q->n    != Q->m)       ESL_EXCEPTION(eslEINVAL, "a rate matrix Q must be square");

  for (i = 0; i < Q->n; i++)
    {
      qi = 0.;
      for (j = 0; j < Q->m; j++)
	{
	  if (i != j) {
	    if (Q->mx[i][j] < 0.)       ESL_FAIL(eslFAIL, errbuf, "offdiag elem %d,%d < 0",i,j);
	    qi += Q->mx[i][j];
	  } else {
	    if (Q->mx[i][j] > 0.)       ESL_FAIL(eslFAIL, errbuf, "diag elem %d,%d < 0", i,j);
	  }
	}
      if (fabs(qi + Q->mx[i][i]) > tol) ESL_FAIL(eslFAIL, errbuf, "row %d does not sum to 0.0", i);
    }
  return eslOK;
}

/*****************************************************************
 * 3. Other routines in the exposed ratematrix API.
 *****************************************************************/

/* Function:  esl_rmx_ScaleTo()
 * Incept:    SRE, Tue Jul 13 16:05:16 2004 [St. Louis]
 *
 * Purpose:   Rescales rate matrix <Q> so that expected substitution
 *            rate per dt is <unit>.
 *
 *            Expected substitution rate is:
 *               $\sum_i \sum_j pi_i Q_ij  \forall i \neq j$
 *
 *            <unit> typically taken to be 1.0, so time units are substitutions/site.
 *            An exception is PAM, where <unit> = 0.01 for 1 PAM unit.
 *
 * Args:      Q     - rate matrix to normalize
 *            pi    - stationary residue frequencies
 *            unit  - expected subsitution rate per dt
 *                    (1.0 = substitutions/site; 0.01 = PAMs)
 *
 * Returns:   <eslOK> on success, and matrix Q is rescaled.
 *
 * Xref:      STL8/p56.
 */
int
esl_rmx_ScaleTo(ESL_DMATRIX *Q, double *pi, double unit)
{
  int     i,j;
  double  sum = 0.;

  if (Q->m != Q->n || Q->type != eslGENERAL)
    ESL_EXCEPTION(eslEINVAL, "Q must be a square general matrix");

  for (i = 0; i < Q->m; i++)
    for (j = 0; j < Q->n; j++)
      if (i != j) sum += pi[i] * Q->mx[i][j];

  for (i = 0; i < Q->m; i++)
    for (j = 0; j < Q->n; j++)
      Q->mx[i][j] *= (unit / sum);

  return eslOK;
}

/* Function:  esl_rmx_E2Q()
 * Incept:    SRE, Tue Jul 13 15:52:41 2004 [St. Louis]
 *
 * Purpose:   Given a lower triangular matrix ($j<i$) of
 *            residue exchangeabilities <E>, and a stationary residue
 *            frequency vector <pi>; assuming $E_{ij} = E_{ji}$;
 *            calculates a rate matrix <Q> as
 *
 *            $Q_{ij} = E_{ij} * \pi_j$
 *
 *            The resulting <Q> is not normalized to any particular
 *            number of substitutions/site/time unit. See
 *            <esl_rmx_ScaleTo()> for that.
 *
 * Args:      E     - symmetric residue "exchangeabilities";
 *                    only lower triangular entries are used.
 *            pi    - residue frequencies at stationarity.
 *            Q     - RETURN: rate matrix, square (NxN).
 *                    Caller allocates the memory for this.
 *
 * Returns:   <eslOK> on success; Q is calculated and filled in.
 *
 * Xref:      STL8/p56.
 */
int
esl_rmx_E2Q(ESL_DMATRIX *E, double *pi, ESL_DMATRIX *Q)
{
  int          i,j;

  if (E->n != Q->n) ESL_EXCEPTION(eslEINVAL, "E and Q sizes differ");

  /* Scale all off-diagonals to pi[j] * E[i][j].
   */
  for (i = 0; i < E->n; i++)
    for (j = 0; j < i; j++)	/* only look at lower triangle of E. */
      {
	Q->mx[i][j] = pi[j] * E->mx[i][j];
	Q->mx[j][i] = pi[i] * E->mx[i][j];
      }

  /* Set diagonal to  -\sum of all j != i.
   */
  for (i = 0; i < Q->n; i++)
    {
      Q->mx[i][i] = 0.;		/* makes the vector sum work for j != i */
      Q->mx[i][i] = -1. * esl_vec_DSum(Q->mx[i], Q->n);
    }
  return eslOK;
}

/* Function:  esl_rmx_RelativeEntropy()
 * Incept:    SRE, Fri Mar 23 09:18:26 2007 [Janelia]
 *
 * Purpose:   Given a conditional substitution probability matrix <P>,
 *            with stationary probabilities <pi>, calculate its
 *            relative entropy $H$:
 *
 *               $H_t = \sum_{ij} P(j \mid i,t) \pi_i \log_2 \frac{P(j \mid i,t)} {\pi_j}$
 *
 *            This assumes that the stationary probabilities are the
 *            same as the background (null model) probabilities.
 *
 * Returns:   the relative entropy, $H$, in bits
 */
double
esl_rmx_RelativeEntropy(ESL_DMATRIX *P, double *pi)
{
  double H = 0.;
  int    i,j;

  for (i = 0; i < P->m; i++)
    for (j = 0; j < P->n; j++)
      H += P->mx[i][j] * pi[i] * log(P->mx[i][j] / pi[j]);
  return H / eslCONST_LOG2;
}

/* Function:  esl_rmx_ExpectedScore()
 * Incept:    SRE, Fri Mar 23 09:32:05 2007 [Janelia]
 *
 * Purpose:   Given a conditional substitution probability matrix <P>
 *            with stationary probabilities <pi>, calculate its
 *            expected score:
 *
 *               $ = \sum_{ij} \pi_j \pi_i \log_2 \frac{P(j \mid i,t)} {\pi_j}$
 *
 *            This assumes that the stationary probabilities are the
 *            same as the background (null model) probabilities.
 *
 * Returns:   the expected score, in bits
 */
double
esl_rmx_ExpectedScore(ESL_DMATRIX *P, double *pi)
{
  double S = 0.;
  int    i,j;

  for (i = 0; i < P->m; i++)
    for (j = 0; j < P->n; j++)
      S += pi[j] * pi[i] * log(P->mx[i][j] / pi[j]);
  return S / eslCONST_LOG2;
}

/*****************************************************************
 * 4. Benchmark driver
 *****************************************************************/

#ifdef eslRATEMATRIX_BENCHMARK

/*
  without GSL:
  gcc -O2 -I. -L. -o benchmark -DeslRATEMATRIX_BENCHMARK esl_ratematrix.c -leasel -lm

  with GSL:
  gcc -g -Wall -I. -L. -o benchmark -DeslRATEMATRIX_BENCHMARK -DHAVE_LIBGSL esl_dmatrix.c esl_ratematrix.c -leasel -lgsl -lgslcblas -lm
 */

#include <esl_config.h>

#ifdef HAVE_LIBGSL
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>
#endif

#include "easel.h"
#include "esl_stopwatch.h"
#include "esl_dmatrix.h"
#include "esl_ratematrix.h"

int
main(void)
{
  ESL_STOPWATCH *w = NULL;
  ESL_DMATRIX *Q  = NULL;
  ESL_DMATRIX *P  = NULL;
  double       t = 5.0;
  int          esl_iterations = 100;
  int          i;
#ifdef HAVE_LIBGSL
  gsl_matrix  *Qg = NULL;
  gsl_matrix  *Pg = NULL;
  int          gsl_iterations = 100;
#endif

  w = esl_stopwatch_Create();
  Q = esl_dmatrix_Create(20, 20);
  P = esl_dmatrix_Create(20, 20);
  esl_rmx_SetWAG(Q, NULL);

  esl_stopwatch_Start(w);
  for (i = 0; i < esl_iterations; i++)
    esl_dmx_Exp(Q, t, P);
  esl_stopwatch_Stop(w);
  printf("Easel takes:   %g sec\n", w->user / (double) esl_iterations);

#ifdef HAVE_LIBGSL
  if (esl_dmx_MorphGSL(Q, &Qg)             != eslOK) esl_fatal("morph to gsl_matrix failed");
  if ((Pg = gsl_matrix_alloc(20, 20))      == NULL)  esl_fatal("gsl alloc failed");
  gsl_matrix_scale(Qg, t);

  esl_stopwatch_Start(w);
  for (i = 0; i < gsl_iterations; i++)
    gsl_linalg_exponential_ss(Qg, Pg, GSL_PREC_DOUBLE);
  esl_stopwatch_Stop(w);
  printf("  GSL takes:   %g sec\n", w->user / (double) gsl_iterations);

  gsl_matrix_free(Qg);
  gsl_matrix_free(Pg);
#endif /*HAVE_LIBGSL*/

  esl_dmatrix_Destroy(Q);
  esl_dmatrix_Destroy(P);
  esl_stopwatch_Destroy(w);
  return 0;
}

#endif /*eslRATEMATRIX_BENCHMARK*/

/*****************************************************************
 * 5. Regression test driver
 *****************************************************************/
#ifdef eslRATEMATRIX_REGRESSION
#ifdef HAVE_LIBGSL

/* This tests rate matrix exponentiation against the GSL's
 * undocumented implementation of a matrix exponential.
 */
/*
  gcc -g -Wall -I. -L. -o ratematrix_regression -DeslRATEMATRIX_REGRESSION -DHAVE_LIBGSL esl_dmatrix.c esl_ratematrix.c -leasel -lgsl -lgslcblas -lm
 */

#include "esl_config.h"

#include <gsl/gsl_matrix.h>
#include <gsl/gsl_linalg.h>

#include "easel.h"
#include "esl_dmatrix.h"
#include "esl_ratematrix.h"

int
main(void)
{
  char errbuf[eslERRBUFSIZE];
  char *alphabet = "ACDEFGHIKLMNPQRSTVWY";
  ESL_DMATRIX *Q  = NULL;
  ESL_DMATRIX *P  = NULL;
  gsl_matrix  *Qg = NULL;
  gsl_matrix  *Pg = NULL;
  ESL_DMATRIX *Pge = NULL;
  double       t = 15.0;

  if ((Q = esl_dmatrix_Create(20, 20))     == NULL)  esl_fatal("malloc failed");
  if ((P = esl_dmatrix_Create(20, 20))     == NULL)  esl_fatal("malloc failed");

  if (esl_rmx_SetWAG(Q, NULL)              != eslOK) esl_fatal("_SetWAG() failed");
  if (esl_rmx_ValidateQ(Q, 0.0001, errbuf) != eslOK) esl_fatal("Q validation failed: %s", errbuf);

  if (esl_dmx_Exp(Q, t, P)                 != eslOK) esl_fatal("matrix exponentiation failed");
  if (esl_rmx_ValidateP(P, 0.0001, errbuf) != eslOK) esl_fatal("P validation failed: %s", errbuf);

  if (esl_dmx_MorphGSL(Q, &Qg)             != eslOK) esl_fatal("morph to gsl_matrix failed");
  if ((Pg = gsl_matrix_alloc(20, 20))      == NULL)  esl_fatal("gsl alloc failed");
  gsl_matrix_scale(Qg, t);
  if (gsl_linalg_exponential_ss(Qg, Pg, GSL_PREC_DOUBLE) != 0) esl_fatal("gsl's exponentiation failed");
  if (esl_dmx_UnmorphGSL(Pg, &Pge)         != eslOK) esl_fatal("morph from gsl_matrix failed");

  esl_dmatrix_Dump(stdout, P, alphabet, alphabet);

  if (esl_dmatrix_Compare(Pge, P, 0.00001) != eslOK) esl_fatal("whoops, different answers.");

  esl_dmatrix_Destroy(Q);
  esl_dmatrix_Destroy(P);
  esl_dmatrix_Destroy(Pge);
  gsl_matrix_free(Qg);
  gsl_matrix_free(Pg);
  return 0;
}
#else
  /* if we don't have GSL, then compile in a dummy main(), solely
   *  to quiet any tests that are verifying that all drivers compile
   *  and run. */
int main(void) { return 0; }
#endif /*HAVE_LIBGSL*/

#endif /*eslRATEMATRIX_REGRESSION*/

/*****************************************************************
 * 6. Unit tests.
 *****************************************************************/
#ifdef eslRATEMATRIX_TESTDRIVE

static void
utest_SetWAG(void)
{
  char errbuf[eslERRBUFSIZE];
  ESL_DMATRIX *Q = NULL;
  ESL_DMATRIX *P = NULL;
  double       t = 50.0;	/* sufficiently large to drive e^tQ to stationarity  */
  double       pi[20];
  int          i;

  if ((Q = esl_dmatrix_Create(20, 20))     == NULL)  esl_fatal("malloc failed");
  if ((P = esl_dmatrix_Create(20, 20))     == NULL)  esl_fatal("malloc failed");

  /* This tests that exponentiating WAG gives a stable conditional
   * probability matrix solution. (It doesn't particularly test that
   * WAG was set correctly, but how could we have screwed that up?)
   */
  if (esl_rmx_SetWAG(Q, NULL)              != eslOK) esl_fatal("_SetWAG() failed");
  if (esl_dmx_Exp(Q, t, P)                 != eslOK) esl_fatal("matrix exponentiation failed");
  if (esl_rmx_ValidateP(P, 1e-7, errbuf)   != eslOK) esl_fatal("P validation failed: %s", errbuf);
  if (esl_rmx_ValidateQ(Q, 1e-7, errbuf)   != eslOK) esl_fatal("Q validation failed: %s", errbuf);

  /* This tests setting WAG to different stationary pi's than default,
   * then tests that exponentiating to large t reaches those stationaries.
   */
  esl_vec_DSet(pi, 20, 0.05);
  if (esl_rmx_SetWAG(Q, pi)                != eslOK) esl_fatal("_SetWAG() failed");
  if (esl_dmx_Exp(Q, t, P)                 != eslOK) esl_fatal("matrix exponentiation failed");
  if (esl_rmx_ValidateP(P, 1e-7, errbuf)   != eslOK) esl_fatal("P validation failed: %s", errbuf);
  if (esl_rmx_ValidateQ(Q, 1e-7, errbuf)   != eslOK) esl_fatal("Q validation failed: %s", errbuf);
  for (i = 0; i < 20; i++)
    if (esl_vec_DCompare(P->mx[i], pi, 20, 1e-7) != eslOK) esl_fatal("P didn't converge to right pi's");

  esl_dmatrix_Destroy(Q);
  esl_dmatrix_Destroy(P);
  return;
}

#ifdef HAVE_LIBLAPACK
static void
utest_Diagonalization(void)
{
  ESL_DMATRIX *P      = NULL;
  ESL_DMATRIX *P2     = NULL;
  ESL_DMATRIX *C      = NULL;
  ESL_DMATRIX *D      = NULL;
  double      *lambda = NULL;		/* eigenvalues */
  ESL_DMATRIX *U      = NULL;		/* left eigenvectors */
  ESL_DMATRIX *Ui     = NULL;		/* inverse of U */
  int  i,j;

  /* Create a J/C probability matrix for t=1:
   *    1/4 + 3/4 e^{-4/3 at}
   *    1/4 - 1/4 e^{-4/3 at}
   */
  if ((P  = esl_dmatrix_Create(4, 4))    == NULL)  esl_fatal("malloc failed");
  if ((C  = esl_dmatrix_Create(4, 4))    == NULL)  esl_fatal("malloc failed");
  if ((Ui = esl_dmatrix_Create(4, 4))    == NULL)  esl_fatal("malloc failed");
  if ((D  = esl_dmatrix_Create(4, 4))    == NULL)  esl_fatal("malloc failed");
  if ((P2 = esl_dmatrix_Create(4, 4))    == NULL)  esl_fatal("malloc failed");
  for (i = 0; i < 4; i++)
    for (j = 0; j < 4; j++)
      if (i == j) P->mx[i][j] = 0.25 + 0.75 * exp(-4./3.);
      else        P->mx[i][j] = 0.25 - 0.25 * exp(-4./3.);

  /* Diagonalize it
   */
  if (esl_dmx_Diagonalize(P, &lambda, NULL, &U, NULL) != eslOK) esl_fatal("diagonalization failed");

  /* Calculate P^k by U [diag(lambda_i)]^k U^{-1}
   */
  esl_dmatrix_SetZero(D);
  for (i = 0; i < P->n; i++) D->mx[i][i] = lambda[i];
  esl_dmx_Invert(U, Ui);
  esl_dmx_Multiply(U, D,  C);
  esl_dmx_Multiply(C, Ui, P2);

  if (esl_dmatrix_Compare(P, P2, 1e-7) != eslOK) esl_fatal("diagonalization unit test failed");

  free(lambda);
  esl_dmatrix_Destroy(P2);
  esl_dmatrix_Destroy(Ui);
  esl_dmatrix_Destroy(U);
  esl_dmatrix_Destroy(D);
  esl_dmatrix_Destroy(C);
  esl_dmatrix_Destroy(P);
  return;
}
#endif /*HAVE_LIBLAPACK*/

#endif /*eslRATEMATRIX_TESTDRIVE*/

/*****************************************************************
 * 7. Test driver
 *****************************************************************/

#ifdef eslRATEMATRIX_TESTDRIVE
/* gcc -g -Wall -o test -I. -L. -DeslRATEMATRIX_TESTDRIVE esl_ratematrix.c -leasel -lm
 * ./test
 *
 * gcc -g -Wall -o test -I. -L. -DHAVE_LIBLAPACK -DeslRATEMATRIX_TESTDRIVE esl_ratematrix.c esl_dmatrix.c -leasel -llapack -lm
 */
#include "esl_config.h"

#include "easel.h"
#include "esl_ratematrix.h"

int
main(void)
{
  utest_SetWAG();
#ifdef HAVE_LIBLAPACK
  utest_Diagonalization();
#endif

  return 0;

}
#endif /*eslRATEMATRIX_TESTDRIVE*/

/*****************************************************************
 * 8. Example driver
 *****************************************************************/

/*****************************************************************
 * Easel - a library of C functions for biological sequence analysis
 * Version h3.1b2; February 2015
 * Copyright (C) 2015 Howard Hughes Medical Institute.
 * Other copyrights also apply. See the COPYRIGHT file for a full list.
 *
 * Easel is distributed under the Janelia Farm Software License, a BSD
 * license. See the LICENSE file for more details.
 *
 * SVN $Id: esl_ratematrix.c 849 2013-01-31 15:10:24Z eddys $
 * SVN $URL: https://svn.janelia.org/eddylab/eddys/easel/branches/hmmer/3.1/esl_ratematrix.c $
 *****************************************************************/


/**
 *
 *	\file gpsins.c - Implements an integrated GPS-Inertial navigation system.
 *
 * 	This program implements an integrated GPS-Inertial navigation system.  Data is
 *	retrieved from shared memory and processed in two threads.  The high speed thread
 *	implements the inertial navigation mechanisation.  The low speed thread combines
 *	the inertial navigation solution with GPS measurements in a tightly coupled
 *	architecture.  The output is stored back into shared memory.
 *
 * 	This program implements a 17 state tightly coupled EKF.  Note
 * 	that the filter states are organised such that the first 15 states
 *	are identical to those used by a 15 state loosely coupled EKF.
 *
 *	The filter uses an error state formulation.  The EKF_x_hat vector contains the state
 *	error rather than the full state.  The full state is stored in regular arrays of
 *	Pos_LLH, Vel_NED and C_BN representing the Latitude, Longitude, Height, North, East
 *	and Down Velocity, and attitude direction cosine matrix (DCM).
 *
 *      \code
 *	Filter States
 *	=============
 *	x1  - Latitude Error (radians)
 * 	x2  - Longitude Error (radians)
 * 	x3  - Height Error (metres)
 * 	x4  - North Velocity Error (m/s)
 * 	x5  - East Velocity Error (m/s)
 *	x6  - Down Velocity Error (m/s)
 * 	x7  - North Tilt Error (radians)
 *	x8  - East Tilt Error (radians)
 *	x9  - Down Tilt Error (radians)
 * 	x10 - X Accelerometer Bias Error (m/s/s)
 *	x11 - Y Accelerometer Bias Error (m/s/s)
 * 	x12 - Z Accelerometer Bias Error (m/s/s)
 * 	x13 - X Gyroscope Bias Error (rad/s)
 * 	x14 - Y Gyroscope Bias Error (rad/s)
 * 	x15 - Z Gyroscope Bias Error (rad/s)
 * 	x16 - Receiver Clock Bias Error (metres)
 * 	x17 - Receiver Clock Frequency Error (metres/sec)
 *
 *	Measurement States - assuming 'n' satellite measurements available
 * 	==================
 * 	z1 - zn - GPS Pseudorange Measurements
 * 	zn+1 - 2zn - GPS Pseudorange Rate (doppler) measurements
 *
 *	These states are not yet implemented...
 *	2nz+1 - Barometric Altitude Error (metres)
 *	2zn+2 - Compass Heading Error (radians)
 *	\endcode
 *
 *	\author Duncan Greer
 *	\version "$Id: gpsins.c 955 2007-12-06 06:38:04Z greerd $"
 */

/* this is an additonal comment */

/* if we are using rtai, declare it here */
/* #define USE_RTAI */
#undef USE_RTAI

#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <strings.h>
#include <string.h>
#include <errno.h>
#include <pthread.h>
#include <stdlib.h>
#include <signal.h>

#ifndef _SVID_SOURCE
#define _SVID_SOURCE
#endif

#include <sys/shm.h>
#include <sys/ipc.h>

#include <mqueue.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_linalg.h>
#include <spawn.h>

#include <sys/mman.h>

#ifdef USE_RTAI
#include <rtai_lxrt.h>
#include <rtai_bits.h>
#else
#include <sys/ioctl.h>
#include <linux/rtc.h>
#endif

#include <math.h>
#include "gpsins.h"
#include "gps.h"
#include "../../novatelstore/src/shmdef.h"
#include "matrix.h"

#define MODULE_NAME 	"[GPSINS]"

#define log(string)	fprintf(stdout,"%s %s\n", MODULE_NAME, string)
#define err(string)	fprintf(stderr,"%s ERROR: %s\n",MODULE_NAME, string)

#define CONTROL_MQUEUE_NAME 	"/control-queue"
#define GPSINS_MQUEUE_NAME 	"/gpsins-queue"
#define	INS_MQUEUE_NAME		"/ins-queue"

#define CMD_SHUTDOWN 1
#define CMD_RUNLOOP 2

#define MAX_DIR_LENGTH 1024

//int Shutdown = 0;

/** thread to control execution of the program */
void *ControlThread(void *ThreadArgs);

/** thread to perform INS solution - 100Hz */
void *INSThread(void *ThreadArgs);

/** thread to perform GPS-INS solution - 1 Hz */
void *GPSINSThread(void *ThreadArgs);

/* INS globals */
double C_BN[3][3];
double Pos_LLH[3];
double Vel_NED[3];
double Acc_NED[3];
double UserClock[2];
double GyroBias[3];
double AccBias[3];

int iShutdown = 0;

/* state variables */
gsl_matrix *EKF_P;
gsl_vector *EKF_x_hat;

/**
 *	\fn HandleSignal
 *	\brief Handle caught signals
 *
 * 	This function sets a flag (iShutdown) to shut the program down.
 */
void HandleSignal(int signal)
{
	fprintf(stdout,"Got Signal: %d\n",signal);
	iShutdown = 1;
}

/**
* main
*/
int main(int argc, char *argv[])
{
	/* generic return value */
	int iReturnValue;

#ifndef USE_RTAI
	int iRTCDevice;
	long lRTCBuffer;
#endif

	/*
	int iMessage = 0;
	char cMessage = 0;
	*/

	message mMessage;

	/* thread paramter/control objects */
	pthread_t ptControlThread;
	pthread_t ptINSThread;
	pthread_t ptGPSINSThread;

	/* pointers to GPS and IMU data respectively */
//	GPSData *ptGPSData;
//	IMUData *ptIMUData;

	/* shared memory objects */
//	int shmGPSId, shmIMUId;
//	key_t shmGPSKey, shmIMUKey;

	/* message queue */
	mqd_t	mqdControl;

	struct mq_attr mqdControlAttr;

	/** GPS INS Thread Message Queue (POSIX) */
	mqd_t	mqdGPSINS;

	/** INS Thread Message Queue (POSIX) */
	mqd_t	mqdINS;

	/** storage for gps store pid */
//	pid_t pidGPSStore;

	/* storage for current working directory */
//	char azcCurrentWorkingDirectory[MAX_DIR_LENGTH];
//	char azcLaunchPath[MAX_DIR_LENGTH+20];
//	char *ptr;

	/* storage for GPSStore Arguments */
//	char *azcGPSStoreArgs[4] = {"gpsstore","/dev/ttyUSB0","230400",NULL};

	sigset_t ssSigSet;

	log("Initialising Program");

	/* setup signal handlers */
	signal(SIGHUP, HandleSignal);
	signal(SIGTERM, HandleSignal);
	signal(SIGKILL, HandleSignal);
	signal(SIGALRM, HandleSignal);
	signal(SIGINT, HandleSignal);

	sigemptyset(&ssSigSet);
	sigaddset(&ssSigSet,SIGRTMIN);
	pthread_sigmask(SIG_BLOCK, &ssSigSet, NULL);

	mqdControlAttr.mq_maxmsg = 10;
	mqdControlAttr.mq_msgsize = sizeof(message);
	mqdControlAttr.mq_flags = 0;

	/*  initialise message	*/
	memset(&mMessage,0,sizeof(message));

	/*  create a control message queue */
	mqdControl = mq_open(CONTROL_MQUEUE_NAME,O_CREAT|O_RDWR|O_EXCL|O_NONBLOCK, (mode_t)0666, &mqdControlAttr);
	if(mqdControl == -1)
	{
		perror("mq_open(): CONTROL_MQUEUE_NAME");
		exit(-1);
	}

	/* setup gps ins message queue */
#ifdef USE_RTAI
	mqdGPSINS = mq_open(GPSINS_MQUEUE_NAME,O_CREAT|O_RDWR|O_EXCL|O_NONBLOCK, (mode_t)0666, &mqdControlAttr);
#else
	mqdGPSINS = mq_open(GPSINS_MQUEUE_NAME,O_CREAT|O_RDWR|O_EXCL, (mode_t)0666, &mqdControlAttr);
#endif
	if(mqdGPSINS == -1)
	{
		perror("mq_open(): GPSINS_MQUEUE_NAME");
		exit(-1);
	}
	/* setup ins message queue */
	mqdINS = mq_open(INS_MQUEUE_NAME,O_CREAT|O_RDWR|O_EXCL|O_NONBLOCK, (mode_t)0666, &mqdControlAttr);
	if(mqdINS == -1)
	{
		perror("mq_open(): INS_MQUEUE_NAME");
		exit(-1);
	}
	/*
	ptmqdControl = (mqd_t *)malloc(sizeof(mqd_t));
	*ptmqdControl = mqdControl;
	*/

	/* start control thread */
	iReturnValue = pthread_create(&ptControlThread,NULL,ControlThread,(void *)&mqdControl);
	if(iReturnValue < 0)
	{
		perror("pthread_create():");
	}

	/* debug message */
	/*
	fprintf(stdout,"%s GPS Data: %d bytes, IMU Data: %d bytes\n",MODULE_NAME, sizeof(GPSData), sizeof(IMUData));
	*/

//	shmGPSKey = GPSDATA_KEY;
//	shmIMUKey = IMUDATA_KEY;

	/*. create shared memory space for storing the data */
//	if((shmGPSId = shmget(shmGPSKey, sizeof(GPSData), IPC_CREAT | 0666)) < 0)
//	{
//      	perror("shmget(): allocating GPS data storage");
//   		exit(-1);
//	}

	/* attach */
//	if((ptGPSData = shmat(shmGPSId, NULL, 0)) == (GPSData *) -1)
//	{
//       	perror("shmat(): attaching GPS Data Storage");
//        	exit(-1);
 //   	}

//	if((shmIMUId = shmget(shmIMUKey,sizeof(GPSData), IPC_CREAT | 0666)) < 0)
//	{
//		perror("shmget(): allocating IMU data storage");
//		exit(-1);
//	}
//	if((ptIMUData = shmat(shmIMUId, NULL, 0)) == (IMUData *)-1)
//	{
//		perror("shmat(): attaching IMU data storage");
//		exit(-1);
//	}

	/* intialise shared memory */
//	memset(ptGPSData,0,sizeof(GPSData));
//	memset(ptIMUData,0,sizeof(IMUData));

	/* get current working directory - used for spawn call */
//	ptr = getcwd(azcCurrentWorkingDirectory,(size_t) MAX_DIR_LENGTH);
//	strcat(azcLaunchPath,azcCurrentWorkingDirectory);
//	strcat(azcLaunchPath,"/");
//	strcat(azcLaunchPath,azcGPSStoreArgs[0]);

	/* start GPS and INS data stores */
//	fprintf(stdout,"%s Launching GPSStore: %s\n",MODULE_NAME,azcLaunchPath);
//	iReturnValue = posix_spawn(&pidGPSStore,		/* storage for child pid */
//		    azcCurrentWorkingDirectory, /* current working directory */
//		    NULL,			/* posix_spawn_file_actions_t */
//		    NULL,			/* posix_spawnattr_t */
//		    azcGPSStoreArgs,		/* GPS Store Args - serial port, baud */
//		    NULL);			/* environment */

//	if(iReturnValue != 0)
//	{
//		perror("posix_spawn(): GPSStore");
//	}

	/* start GPS and INS threads  */
	iReturnValue = pthread_create(&ptINSThread,NULL,INSThread,(void *)&mqdINS);
	if(iReturnValue < 0)
	{
		perror("pthread_create(): INS Thread");
	}

	iReturnValue = pthread_create(&ptGPSINSThread,NULL,GPSINSThread,(void *)&mqdGPSINS);
	if(iReturnValue < 0)
	{
		perror("pthread_create(): GPS-INS Thread");
	}

#ifndef USE_RTAI
	/* open real time clock if we are not using RTAI */
	iRTCDevice = open("/dev/rtc",O_RDONLY);
	if (iRTCDevice < 0)
	{
		perror("open: RTC: ");
	} else
	{
		ioctl(iRTCDevice,RTC_UIE_ON,NULL);
		log("Successfully opened RTC");
	}
#endif
	while(iShutdown == 0)
	{
		iReturnValue = mq_receive(mqdControl,(char *)&mMessage,sizeof(message),NULL);
		if(iReturnValue < 0)
		{
			/*
				perror("mq_receive():");
			*/
		}
		/*
		log("Received Message");
		fprintf(stdout,"Rx: %d, %d\n",iReturnValue, mMessage.iMessageType);
		*/
		if(mMessage.iMessageType == 1)
		{
			// exit
			log("Received Shutdown Message");
			iShutdown = 1;
			break;
		}

		/* this read will return at 1 second intervals */
		read(iRTCDevice,&lRTCBuffer,sizeof(long));

		/* send message to run threads */
		mMessage.iMessageType = CMD_RUNLOOP;
		iReturnValue = mq_send(mqdGPSINS,(const char *)&mMessage,sizeof(message),1);

	}

	log("Shutting Down");

	/* send message to shutdown threads */
	mMessage.iMessageType = CMD_SHUTDOWN;
	iReturnValue = mq_send(mqdGPSINS,(const char *)&mMessage,sizeof(message),1);
	if(iReturnValue < 0)
	{
		perror("mq_send(): Thread Shutdown");
	}
	iReturnValue = mq_send(mqdINS,(const char *)&mMessage,sizeof(message),1);
	if(iReturnValue < 0)
	{
		perror("mq_send(): Thread Shutdown");
	}

	iReturnValue = mq_send(mqdControl,(const char *)&mMessage,sizeof(message),1);
	if(iReturnValue < 0)
	{
		perror("mq_send(): Thread Shutdown");
	}

	/* wait for threads to shutdown */
	//sleep(2);
	fprintf(stdout,"%s Waiting for INS thread to join...", MODULE_NAME);
	pthread_join(ptINSThread,NULL);
	fprintf(stdout,"DONE\n");

	fprintf(stdout,"%s Waiting for GPS-INS thread to join...", MODULE_NAME);
	pthread_join(ptGPSINSThread,NULL);
	fprintf(stdout,"DONE\n");

	fprintf(stdout,"%s Waiting for Control thread to join...", MODULE_NAME);
	pthread_join(ptControlThread,NULL);
	fprintf(stdout,"DONE\n");

#ifndef USE_RTAI
	/* close rtc */
	close(iRTCDevice);
#endif

	/* close and unlink message queues */
	fprintf(stdout,"%s Unlinking Message Queues...",MODULE_NAME);
	mq_close(mqdControl);
	mq_unlink(CONTROL_MQUEUE_NAME);
	mq_close(mqdGPSINS);
	mq_unlink(GPSINS_MQUEUE_NAME);
	mq_close(mqdINS);
	mq_unlink(INS_MQUEUE_NAME);

	fprintf(stdout,"DONE\n");

	log("Exiting");
	pthread_exit(NULL);
	return 0;
} /* end main */

/*
* ControlThread - controls execution of the program
*/
void *ControlThread(void *ThreadArgs)
{
	char c[100];
	mqd_t *ptmqdControl;
	int iReturnValue;

	message mMessage;

	fprintf(stdout,"%s Control Thread Initialised - press 'q <enter>' to exit!\n",MODULE_NAME);

	/* initialise message	 */
	memset(&mMessage,0,sizeof(message));

	/* read arguments */
	ptmqdControl = (mqd_t *)ThreadArgs;

	while(scanf("%s",c))
	{
		/* check for message (non blocking) */
		iReturnValue = mq_receive(*ptmqdControl,(char *)&mMessage,sizeof(message),NULL);
		if(iReturnValue < 0)
		{
			perror("mq_recieve(): Control Thread");
		}

		if(mMessage.iMessageType == CMD_SHUTDOWN)
		{
			log("Control Thread Received SHUTDOWN command");
			//pthread_exit(NULL);
			break;
		}

		switch(strlen(c))
		{
			case 1:
				/* command character */
				switch(c[0])
				{
					case 'q':
						mMessage.iMessageType = CMD_SHUTDOWN;

						/* send shutdown message */
						iReturnValue = mq_send(*ptmqdControl,
								(const char *)&mMessage,
								sizeof(message),
								1);
						if(iReturnValue < 0)
						{
							perror("mq_send():");
						}
						break;

					default:
						fprintf(stderr,"%s Unknown command: %c\n",MODULE_NAME, c[0]);
						break;
				}
			default:
				/* do nothing */
				break;
		}

	}

	fprintf(stdout,"%s Control Thread Exiting\n", MODULE_NAME);

	/* never reached */
	pthread_exit(NULL);
}

int iINSThreadCounter;

void *INSThread(void *ThreadArgs)
{
	/* message storage */
	message mMessage;

	/* generic return storage */
	int iReturnValue;

	/* message queue pointer */
	mqd_t *ptmqdINS;

	/* timer variables */
	int iSigNumber;
	sigset_t ssSigSet;
	struct sigevent seSigEvent;
	timer_t tPeriodicTimer;
	struct timespec first,period;
	struct itimerspec required,old;
	long lThreadPeriod = 10000000; /* 10ms = 100 hz */

	/* shared memory variables */
	int iUseSHM = 1;
	shm_struct *shm = NULL;
	int iSHM_FID;

	double EarthRate[3];
	double Tecef2ned[3][3];
	double RM,RP;
	double Lat,Long;
	double LatDot,LongDot;
	double sLat,cLat;
	double OMEGA_b[3][3];
	double Coriolis[3];
	double Acc_XYZ[3];
	double omega_x,omega_y,omega_z;
	double OMEGA_e_n[3];
	double OMEGA_in[3][3];

	fprintf(stdout,"%s INS Thread Started\n",MODULE_NAME);

	/* initialise message	*/
	memset(&mMessage,0,sizeof(message));

	/* setup message queue */
	ptmqdINS = (mqd_t *)ThreadArgs;

	/* setup thread timer */
	seSigEvent.sigev_notify = SIGEV_SIGNAL;
	seSigEvent.sigev_signo = SIGRTMIN;

	clock_gettime(CLOCK_REALTIME, &first);
	first.tv_sec = first.tv_sec + 1;

	fprintf(stdout,"INS: First loop runs at %ld.%ld\n", (long)first.tv_sec,(long)first.tv_nsec);

	period.tv_sec = 0;
	period.tv_nsec = lThreadPeriod;

	required.it_value = first;
	required.it_interval = period;

	iReturnValue = timer_create(CLOCK_REALTIME,&seSigEvent,&tPeriodicTimer);
	if(iReturnValue < 0)
	{
		perror("INS: timer_create():");
	}

	sigemptyset(&ssSigSet);
	sigaddset(&ssSigSet,SIGRTMIN);
	//pthread_sigmask(SIG_UNBLOCK, &ssSigSet, NULL);

	iReturnValue = timer_settime(tPeriodicTimer,TIMER_ABSTIME,&required, &old);
	if(iReturnValue < 0)
	{
		perror("INS: timer_settime():");
	}

       	/* setup shared memory */
	if(iUseSHM == 1)
	{
	  iSHM_FID = shm_open("/novatel0",O_RDWR,0777);
	  if(iSHM_FID == -1)
	  {
		perror("shm_open: ");
		iUseSHM = 0;
 	  } else
	  {
	     	/* memory map the shm object */
	     	shm = mmap(0,sizeof(*shm),PROT_READ,MAP_SHARED,iSHM_FID,0);
	     	if(shm == MAP_FAILED)
	     	{
		   perror("shm mmap: ");
		   iUseSHM = 0;
	     	} else
		{
		  fprintf(stdout,"%s SHM Address Is: 0x%08x\n",MODULE_NAME,(unsigned int)shm);
		}
	  }
	}

	/* initialise INS parameters */
	C_BN[0][0] = 1.0;
	C_BN[1][1] = 1.0;
	C_BN[2][2] = 1.0;

	GyroBias[0] = 0.0;
	GyroBias[1] = 0.0;
	GyroBias[2] = 0.0;

	AccBias[0] = 0.0;
	AccBias[1] = 0.0;
	AccBias[2] = 0.0;

	/* position */
	Pos_LLH[0] = -27.5*DEG2RAD;
	Pos_LLH[1] = 153.0*DEG2RAD;
	Pos_LLH[2] = 90.0;

	/* velocity */
	Vel_NED[0] = 0.0;
	Vel_NED[1] = 0.0;
	Vel_NED[2] = 0.0;

	/* acceleration */
	Acc_NED[0] = 0.0;
	Acc_NED[1] = 0.0;
	Acc_NED[2] = -LOCAL_GRAVITY;

	Acc_XYZ[0] = 0.0;
	Acc_XYZ[1] = 0.0;
	Acc_XYZ[2] = -LOCAL_GRAVITY;

	EarthRate[0] = 0.0;
	EarthRate[1] = 0.0;
	EarthRate[2] = OMEGA_e;

	Coriolis[0] = 0.0;
	Coriolis[1] = 0.0;
	Coriolis[2] = 0.0;

	RM = WGS84_a;
	RP = WGS84_a;
	WGS84_calcRnRe(Pos_LLH[0],  &RM, &RP);

	OMEGA_e_n[0] = 0.0;
	OMEGA_e_n[1] = 0.0;
	OMEGA_e_n[2] = 0.0;

	log("INS Thread Commencing Periodic");

	while(1)
	{

		//fprintf(stdout,"Waiting...");
		/* wait for signal */
		sigwait(&ssSigSet, &iSigNumber);

		iINSThreadCounter++;

		/* check for messages */
		iReturnValue = mq_receive(*ptmqdINS,(char *)&mMessage,sizeof(message),NULL);
		if(iReturnValue < 0 && errno != EAGAIN)
		{
			perror("INS: mq_recieve(): ");
			//fprintf(stdout,"%d",iReturnValue);
		}

		if(mMessage.iMessageType == CMD_SHUTDOWN)
		{
			break;
		}

		/* get current lat and long */
		Lat = Pos_LLH[0];
		Long = Pos_LLH[1];
		cLat = cos(Lat);
		sLat = sin(Lat);

		T_ECEF2NED(Lat, Long, (double **)Tecef2ned);

		/* attitude update */
		omega_x = shm->CurrentIMUData.dRate[0] - GyroBias[0];
		omega_y = shm->CurrentIMUData.dRate[1]  - GyroBias[1];
		omega_z = shm->CurrentIMUData.dRate[2]  - GyroBias[2];

		/* form the skew symmetric form */
		OMEGA_b[0][0] = 0.0;
		OMEGA_b[0][1] = -omega_z;
		OMEGA_b[0][2] =  omega_y;
		OMEGA_b[1][0] =  omega_z;
		OMEGA_b[1][1] = 0.0;
		OMEGA_b[1][2] = -omega_x;
		OMEGA_b[2][0] = -omega_y;
		OMEGA_b[2][1] = omega_x;
		OMEGA_b[2][2] = 0.0;

		/* navigation frame rotation correction */
		OMEGA_in[0][0] = 0.0;
		OMEGA_in[0][1] = -OMEGA_e * sLat;
		OMEGA_in[0][2] = 0.0;
		OMEGA_in[1][0] = OMEGA_e * sLat;
		OMEGA_in[1][1] = 0.0;
		OMEGA_in[1][2] = -OMEGA_e * cLat;
		OMEGA_in[2][0] = 0.0;
		OMEGA_in[2][1] = OMEGA_e * cLat;
		OMEGA_in[2][2] = 0.0;

		/* DCM Update */
		/* C_BN = C_BN + (C_BN * OMEGA_b - OMEGA_in * C_BN)*INS_DT */

		/* acceleration calculation */
		/* Acc_NED = C_BN * Acc_XYZ */
		Acc_XYZ[0] = shm->CurrentIMUData.dAcc[0] - AccBias[0];
		Acc_XYZ[1] = shm->CurrentIMUData.dAcc[1] - AccBias[1];
		Acc_XYZ[2] = shm->CurrentIMUData.dAcc[2] - AccBias[2] -LOCAL_GRAVITY;

		MatMul331(C_BN, Acc_XYZ, Acc_NED);

		/* correct vertical channel for gravity */
		Acc_NED[2] = Acc_NED[2] + LOCAL_GRAVITY;

		/* coriolis correction */
		MatMul331(Tecef2ned, EarthRate, OMEGA_e_n);

		/* Coriolis = cross(2 * OMEGA_e_n,[0,0,0]); */

		Acc_NED[0] = Acc_NED[0] - Coriolis[0];
		Acc_NED[1] = Acc_NED[1] - Coriolis[1];
		Acc_NED[2] = Acc_NED[2] - Coriolis[2];

		/* velocity update */
		Vel_NED[0] = Vel_NED[0] + Acc_NED[0] * INS_DT;
		Vel_NED[1] = Vel_NED[1] + Acc_NED[1] * INS_DT;
		Vel_NED[2] = Vel_NED[2] + Acc_NED[2] * INS_DT;

		WGS84_calcRnRe(Pos_LLH[0],  &RM, &RP);
		LatDot = Vel_NED[0] / (RM + Pos_LLH[2]);
		LongDot = Vel_NED[1] / (cos(Pos_LLH[0])*(RP + Pos_LLH[2]));

		/* position update */
		Pos_LLH[0] = Pos_LLH[0] + LatDot * INS_DT;
		Pos_LLH[1] = Pos_LLH[1] + LongDot * INS_DT;
		Pos_LLH[2] = Pos_LLH[2] + -Vel_NED[2] * INS_DT;

		/* covariance update */
		/* P = PHI * P * PHI' + Qd */

	}
	/* exit */
	fprintf(stdout,"%s INS Thread Exiting\n",MODULE_NAME);
	pthread_exit(NULL);
}

void *GPSINSThread(void *ThreadArgs)
{
	int iRunThread = 1;

	/* pointer to message queue designator passed from arguments */
	mqd_t *ptmqdGPSINS;

	/* generic return value storage */
	int iReturnValue;

	/* generic message storage */
	message mMessage;

#ifdef USE_RTAI
	/* real time task */
	static RT_TASK *rttGPSINS;
#endif

	/* KF stuff */
	int iNumberStates,iNumberMeasurements;

	/* matrices */
	gsl_matrix *EKF_Q;
	gsl_matrix *EKF_PHI; /* phi ~= I + Fdt */
	gsl_matrix *EKF_F;
	gsl_matrix *EKF_P_minus;
	gsl_matrix *tempP;

	gsl_matrix *EKF_H;
	gsl_matrix *EKF_R;
	gsl_matrix *EKF_K;
	gsl_matrix *EKF_V;
	gsl_matrix *EKF_Vinv;
	gsl_matrix *tempV;
	gsl_matrix *tempK;

	gsl_matrix *KH;
	gsl_matrix *I_17by17;
	gsl_matrix *IminusKH;
	gsl_matrix *KHPminus;
	gsl_matrix *KRKT;
	gsl_matrix *tempKRKT;

	/* svd stuff */
	gsl_vector *SVD_S;
	gsl_vector *SVD_Sinv;
	gsl_matrix *SVD_SinvMat;
	gsl_vector *SVDWork;
	gsl_matrix *SVD_V;
	gsl_matrix *SVD_U;
	gsl_matrix *tempNN;

	/* vectors */
	gsl_vector *EKF_z;
	gsl_vector *EKF_deltaz;
	gsl_vector *EKF_x_hat_out;

	gsl_vector *temp1;

	double x_gyro_beta;
	double y_gyro_beta;
	double z_gyro_beta;
	double x_accel_beta;
	double y_accel_beta;
	double z_accel_beta;

	double user_x,user_y,user_z,user_t;
	double user_xdot,user_ydot,user_zdot,user_tdot;
	double sat_x,sat_y,sat_z,sat_t;
	double sat_xdot,sat_ydot,sat_zdot,sat_tdot;
	double ION_Alpha[4];
	double ION_Beta[4];
	unsigned short usPRN;
	double range,rangerate;
	double geo_vel_to_sat;
	double delta_pr_omegaedot;

	double Rm,Rp;
	double Rmh,Rph,Rg;

	double IonoDelay[NOVATEL_MAXCHANNELS],TropoDelay[NOVATEL_MAXCHANNELS];
	double SV_Azimuth[NOVATEL_MAXCHANNELS];
	double SV_Elevation[NOVATEL_MAXCHANNELS];
//	double llh[3];
//	double ecef[3];
	double dTransmissionTime;

	GPSTime gtObservationTime;
	RANGE	GPSRangeData;
	GPSEPHEM GPSEphemData[NOVATEL_MAXPRNS];
	int iIndex;
	int iNumberValidObservations;
	int iNumberValidEphemeris;
	int iUseSV[NOVATEL_MAXCHANNELS];

	double UserPos[3];
	double UserVel[3];

	double Tecef2ned[3][3];
	double Tned2ecef[3][3];

	double SVPos_calc[4];
	double SVVel_calc[4];
	double SVAcc_calc[4];

	double C_BN_new[3][3];

	double EKF_H_E[3][3];
	double EKF_H_LTP[3][3];

	double del_alpha,del_beta,del_gamma;
	double del_att_skew[3][3];

	/* matrix counter variables */
	int iRowIndex,iColIndex;

	/* shared memory variables */
	int iUseSHM = 1;
	shm_struct *shm = NULL;
	int iSHM_FID;

	/* log message */
	fprintf(stdout,"%s GPS-INS Thread Started\n",MODULE_NAME);

	/* initialise message	 */
	memset(&mMessage,0,sizeof(message));

	/* setup message queue */
	ptmqdGPSINS = (mqd_t *)ThreadArgs;

	/* setup EKF */
	iNumberStates = NUMBER_STATES;
	iNumberMeasurements = 0;

	EKF_x_hat = gsl_vector_alloc(iNumberStates);
	EKF_x_hat_out = gsl_vector_alloc(iNumberStates);

	EKF_Q = gsl_matrix_alloc(iNumberStates,iNumberStates);
	EKF_P = gsl_matrix_alloc(iNumberStates,iNumberStates);
	EKF_P_minus = gsl_matrix_alloc(iNumberStates,iNumberStates);
	tempP = gsl_matrix_alloc(iNumberStates,iNumberStates);

	EKF_PHI = gsl_matrix_alloc(iNumberStates,iNumberStates);
	EKF_F = gsl_matrix_alloc(iNumberStates,iNumberStates);

	I_17by17 = gsl_matrix_alloc(iNumberStates,iNumberStates);

	KH = gsl_matrix_alloc(iNumberStates,iNumberStates);
	IminusKH = gsl_matrix_alloc(iNumberStates,iNumberStates);
	KHPminus = gsl_matrix_alloc(iNumberStates,iNumberStates);

	gsl_matrix_set_identity(EKF_P);
	gsl_matrix_scale(EKF_P,10.0);

	temp1 = gsl_vector_alloc(3);

	x_gyro_beta = 1/100.0;
	y_gyro_beta = 1/100.0;
	z_gyro_beta = 1/100.0;

	x_accel_beta = 1/100.0;
	y_accel_beta = 1/100.0;
	z_accel_beta = 1/100.0;

	UserClock[0] = 0.0;
	UserClock[1] = 0.0;

#ifdef USE_RTAI
	/* make this thread real time */
	rt_set_oneshot_mode();
	start_rt_timer(0);
	rttGPSINS = rt_task_init_schmod(nam2num("GPSINS"),
				10,
				0,
				0,
				SCHED_FIFO,
				0);
 	if (rttGPSINS == NULL)
	{
		perror("rt_task_init(): GPSINS");
		exit(-1);
	}

	rt_make_hard_real_time();
	rt_task_make_periodic(rttGPSINS,
		rt_get_time()+nano2count(1000000000)*2,
		nano2count(1000000000));

#endif

       	/* setup shared memory */
	if(iUseSHM == 1)
	{
	  iSHM_FID = shm_open("/novatel0",O_RDWR,0777);
	  if(iSHM_FID == -1)
	  {
		perror("shm_open: ");
		iUseSHM = 0;
 	  } else
	  {
	     	/* memory map the shm object */
	     	shm = mmap(0,sizeof(*shm),PROT_READ,MAP_SHARED,iSHM_FID,0);
	     	if(shm == MAP_FAILED)
	     	{
		   perror("shm mmap: ");
		   iUseSHM = 0;
	     	} else
		{
		  fprintf(stdout,"%s SHM Address Is: 0x%08x\n",MODULE_NAME,(unsigned int)shm);
		}
	  }
	}

	/* reset INS thread counter */
	iINSThreadCounter = 0;

	while(iRunThread == 1)
	{
		/* wait for message */ /* non blocking  if using RTAI*/
		iReturnValue = mq_receive(*ptmqdGPSINS,(char *)&mMessage,sizeof(message),NULL);
		if(iReturnValue < 0)
		{
			/* check value of errno */
			switch(errno)
			{
				case EAGAIN:
					/* return with no message - do nothing */
					break;
				default:
					perror("mq_recieve(): GPSINSThread");
					break;
			}

		}

// 		if(mMessage.iMessageType == CMD_SHUTDOWN)
// 		{
// 			break;
// 		}

		switch(mMessage.iMessageType)
		{
		  case CMD_RUNLOOP:
			break;

		  case CMD_SHUTDOWN:
			iRunThread = 0;
			break;

		  default:
			err("GPSINSThread Unknown Message");
			break;
		}

#ifdef USE_RTAI
		/* wait until its time to run */
		rt_task_wait_period();
#endif
//		fprintf(stdout,".\n");
		fprintf(stdout,"%s: GPSINS Counter: %d\n",MODULE_NAME,iINSThreadCounter);

		/* RUN PERIODIC TASKS HERE */

		/* get INS update from store */

		T_ECEF2NED(Pos_LLH[0],Pos_LLH[1],(double **)Tecef2ned);

		/* get GPS measurements */
		memcpy(&GPSEphemData,&shm->CurrentGPSEPHEM,sizeof(GPSEPHEM)*NOVATEL_MAXPRNS);

	 	// get the current observation time to compare with ephemeris
	 	memcpy(&gtObservationTime,&shm->CurrentRANGE.gtTimeStamp,sizeof(GPSTime));

		/* ensure we have ephemeris for all measurements, otherwise exclude */
		iNumberValidEphemeris = 0;
		for(iIndex=0;iIndex<NOVATEL_MAXPRNS;iIndex++)
		{
			if((GPSEphemData[iIndex].ulPRN != 0) &&
		  	 (GPSEphemData[iIndex].ulHealth == 0) &&
		  	 (GPSEphemData[iIndex].dTOE < (gtObservationTime.fGPSSecondOfWeek+7500.0)) &&
		  	 (GPSEphemData[iIndex].dTOE > (gtObservationTime.fGPSSecondOfWeek-7500.0))
		  	 )
		  	 {
		  	 	iUseSV[iIndex] = 1;
		  	 	iNumberValidEphemeris++;
	  		} else
		  	{
		  	  iUseSV[iIndex] = 0;
		  	}
		}

		/* loop through measurements and see
		     if we have valid ephem for each */

		memcpy(&GPSRangeData.gtTimeStamp,&shm->CurrentRANGE.gtTimeStamp,sizeof(GPSTime));
		iNumberValidObservations = 0;

		for(iIndex=0;iIndex<shm->CurrentRANGE.lNumberObservations;iIndex++)
		{
	  		if(iUseSV[shm->CurrentRANGE.usPRN[iIndex]] == 1)
	  		{
	  			// use current range data for now
	  			memcpy(&GPSRangeData.usPRN[iNumberValidObservations],
				       &shm->CurrentRANGE.usPRN[iIndex],sizeof(unsigned short));
				memcpy(&GPSRangeData.dPseudorange[iNumberValidObservations],
				       &shm->CurrentRANGE.dPseudorange[iIndex], sizeof(double));

				iNumberValidObservations++;
	  		} else
	  		{
	  		  fprintf(stdout,"NE: %d; ",shm->CurrentRANGE.usPRN[iIndex]);
	  		}
		}

		fprintf(stdout,"Obs: %d; \n",iNumberValidObservations);
		GPSRangeData.lNumberObservations = iNumberValidObservations;

		iNumberMeasurements = GPSRangeData.lNumberObservations;

		if(iNumberMeasurements < 4)
		{
			log("GPSINS Less than 4 Measurements.... ");
			if(iRunThread == 1)
			{
				log("GPSINS Continuing...");
				continue;
			} else
			{
				log("GPSINS Exiting..");
				break;
			}
		}

		fprintf(stdout,"Allocating Matrices\n");

		EKF_z = gsl_vector_alloc(2*iNumberMeasurements);
		EKF_deltaz = gsl_vector_alloc(2*iNumberMeasurements);
		EKF_R = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		EKF_H = gsl_matrix_alloc(2*iNumberMeasurements,iNumberStates);
		tempV = gsl_matrix_alloc(2*iNumberMeasurements,iNumberStates);
		EKF_V = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		EKF_Vinv = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		EKF_K = gsl_matrix_alloc(iNumberStates,2*iNumberMeasurements);
		tempK = gsl_matrix_alloc(iNumberStates,2*iNumberMeasurements);
		tempKRKT = gsl_matrix_alloc(iNumberStates,2*iNumberMeasurements);
		KRKT = gsl_matrix_alloc(iNumberStates,iNumberStates);

		SVDWork = gsl_vector_alloc(2*iNumberMeasurements);
		SVD_S = gsl_vector_alloc(2*iNumberMeasurements);
		SVD_U = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		SVD_SinvMat = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		tempNN = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		SVD_V = gsl_matrix_alloc(2*iNumberMeasurements,2*iNumberMeasurements);
		SVD_Sinv = gsl_vector_alloc(2*iNumberMeasurements);

		/* now that all our memory is allocated, disable paging */
//		mlockall(MCL_CURRENT | MCL_FUTURE);

//		user_x = gsl_vector_get(EKF_x_hat,0);
//		user_y = gsl_vector_get(EKF_x_hat,1);
//		user_z = gsl_vector_get(EKF_x_hat,2);
//		user_t = gsl_vector_get(EKF_x_hat,15);

//		UserPos[0] = user_x;
//		UserPos[1] = user_y;
//		UserPos[2] = user_z;
//		UserPos[3] = user_t;

		WGS84_LLH2ECEF(Pos_LLH, UserPos);
		user_x = UserPos[0];
		user_y = UserPos[1];
		user_z = UserPos[2];
		user_t = UserClock[0];

//		user_xdot = gsl_vector_get(EKF_x_hat,3);
//		user_ydot = gsl_vector_get(EKF_x_hat,4);
//		user_zdot = gsl_vector_get(EKF_x_hat,5);
//		user_tdot = gsl_vector_get(EKF_x_hat,16);

//		UserVel[0] = user_xdot;
//		UserVel[1] = user_ydot;
//		UserVel[2] = user_zdot;
//		UserVel[3] = user_tdot;

		T_NED2ECEF(Pos_LLH[0], Pos_LLH[1], (double **)Tned2ecef);

		MatMul331(Tned2ecef, Vel_NED, UserVel);
		user_xdot = UserVel[0];
		user_ydot = UserVel[1];
		user_zdot = UserVel[2];
		user_tdot = UserClock[1];

		ION_Alpha[0] = shm->CurrentIONUTC.a0;
		ION_Alpha[1] = shm->CurrentIONUTC.a1;
		ION_Alpha[2] = shm->CurrentIONUTC.a2;
		ION_Alpha[3] = shm->CurrentIONUTC.a3;

		ION_Beta[0] = shm->CurrentIONUTC.b0;
		ION_Beta[1] = shm->CurrentIONUTC.b1;
		ION_Beta[2] = shm->CurrentIONUTC.b2;
		ION_Beta[3] = shm->CurrentIONUTC.b3;

		//	fprintf(stdout,"User: [%+10.3f\t%+10.3f\t%+10.3f\t%+10.3f]\n",user_x,user_y,user_z,user_t);
		fprintf(stdout,"Organising Measurements\n");

		for (iIndex=0;iIndex<iNumberMeasurements;iIndex++)
		{

			dTransmissionTime = (double)gtObservationTime.fGPSSecondOfWeek
				- GPSRangeData.dPseudorange[iIndex]/GPS_SPEEDOFLIGHT;

			GPSOrbitPropagator(dTransmissionTime,
					GPSRangeData.usPRN[iIndex],
					&GPSEphemData[GPSRangeData.usPRN[iIndex]],
					7500.0,
					SVPos_calc);

			GPSOrbitPropagatorVelocities(dTransmissionTime,
						GPSRangeData.usPRN[iIndex],
						&GPSEphemData[GPSRangeData.usPRN[iIndex]],
						7500.0,
						SVVel_calc,
						SVAcc_calc);

			usPRN = GPSRangeData.usPRN[iIndex];

			sat_x = SVPos_calc[0];
			sat_y = SVPos_calc[1];
			sat_z = SVPos_calc[2];
			sat_t = SVPos_calc[3]*GPS_SPEEDOFLIGHT;

			sat_xdot = SVVel_calc[0];
			sat_ydot = SVVel_calc[1];
			sat_zdot = SVVel_calc[2];
			sat_tdot = SVVel_calc[3];

			// find the range
			gsl_vector_set(temp1,0,sat_x - user_x);
			gsl_vector_set(temp1,1,sat_y - user_y);
			gsl_vector_set(temp1,2,sat_z - user_z);

			range = gsl_blas_dnrm2(temp1);

			/* calculate the range rate (relative velocity) to the SV */
			geo_vel_to_sat =  (sat_xdot - user_xdot)*(sat_x-user_x)
				  	+ (sat_ydot - user_ydot)*(sat_y-user_y)
					+ (sat_zdot - user_zdot)*(sat_z-user_z);

			rangerate = geo_vel_to_sat / range;

			EKF_H_E[iIndex][0] = -(sat_x - user_x)/range;
			EKF_H_E[iIndex][1] = -(sat_y - user_y)/range;
			EKF_H_E[iIndex][2] = -(sat_z - user_z)/range;

//			gsl_matrix_set(EKF_H,iIndex,0,-(sat_x - user_x)/range);
//			gsl_matrix_set(EKF_H,iIndex,1,-(sat_y - user_y)/range);
//			gsl_matrix_set(EKF_H,iIndex,2,-(sat_z - user_z)/range);
//			gsl_matrix_set(EKF_H,iIndex,15,1.0);

//			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,3,-(sat_x - user_x)/range);
//			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,4,-(sat_y - user_y)/range);
//			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,5,-(sat_z - user_z)/range);
//			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,16,1.0);

			gsl_vector_set(EKF_z,iIndex,GPSRangeData.dPseudorange[iIndex]);
			// calculate earth rotation correction
			delta_pr_omegaedot = -(GPS_OMEGAEDOT/GPS_SPEEDOFLIGHT) * (sat_x*user_y - sat_y*user_x);

			// calculate iono correction
			ICD200_IonoModel(gtObservationTime,
					UserPos,
					SVPos_calc,
					ION_Alpha,
					ION_Beta,
					&IonoDelay[iIndex],
					&SV_Azimuth[iIndex],
					&SV_Elevation[iIndex]);

			TropoDelay[iIndex] = TropoDelayModel(SV_Elevation[iIndex],Pos_LLH[3]);

			/* set PR measurement */
			gsl_vector_set(EKF_deltaz,iIndex,
				GPSRangeData.dPseudorange[iIndex] - range
				- (user_t-sat_t)
				+ delta_pr_omegaedot
				- IonoDelay[iIndex]
				- TropoDelay[iIndex]);

//			fprintf(stdout,"%f %f %f %f %f %f\n",
//				range,
//				user_t,
//				sat_t,
//				delta_pr_omegaedot,
//				IonoDelay[iIndex],
//				TropoDelay[iIndex]);

			/* set doppler measurement */
			gsl_vector_set(EKF_deltaz,iIndex+iNumberMeasurements,
				-(double)GPSRangeData.fDoppler[iIndex]*GPS_L1_Wavelength
				- rangerate
				- (user_tdot));

		}  // end for i=1:NumberMeasurements

		// rotate H matrix to local
		MatMul333(Tecef2ned,EKF_H_E,EKF_H_LTP);

		for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
		{
			gsl_matrix_set(EKF_H,iIndex,0,EKF_H_LTP[iIndex][0]);
			gsl_matrix_set(EKF_H,iIndex,1,EKF_H_LTP[iIndex][1]);
			gsl_matrix_set(EKF_H,iIndex,2,EKF_H_LTP[iIndex][2]);
			gsl_matrix_set(EKF_H,iIndex,15,1.0);

			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,3,EKF_H_LTP[iIndex][0]);
			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,4,EKF_H_LTP[iIndex][1]);
			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,5,EKF_H_LTP[iIndex][2]);
			gsl_matrix_set(EKF_H,iIndex+iNumberMeasurements,16,1.0);

		}

//		gsl_vector_fprintf(stdout,EKF_deltaz,"[%f]");
//		gsl_matrix_fprintf(stdout,EKF_H,"%f");

		fprintf(stdout,"Setting up PHI matrix\n");
		WGS84_calcRnRe(Pos_LLH[0],  &Rm, &Rp);
		Rmh = Rm+Pos_LLH[3];
		Rph = Rp+Pos_LLH[3];
		Rg = sqrt(Rmh*Rmh + Rph*Rph);

		/* kalman filter */
		gsl_matrix_set_zero(EKF_F);
		gsl_matrix_set(EKF_F,0,3,1.0/Rmh);
		gsl_matrix_set(EKF_F,1,4,1.0/(Rph*cos(Pos_LLH[0])));
		gsl_matrix_set(EKF_F,2,5,-1.0);
		gsl_matrix_set(EKF_F,0,2,-Vel_NED[0]/(Rg*Rg));
		gsl_matrix_set(EKF_F,1,0,Vel_NED[1]*sin(Pos_LLH[0])/(Rg*cos(Pos_LLH[0])*cos(Pos_LLH[0])));
		gsl_matrix_set(EKF_F,1,2,-Vel_NED[1]/(Rg*Rg*cos(Pos_LLH[0])));
		gsl_matrix_set(EKF_F,1,4,1.0/(Rg*cos(Pos_LLH[0])));
		gsl_matrix_set(EKF_F,3,0,-Vel_NED[1]*2.0*OMEGA_e*cos(Pos_LLH[0]) - Vel_NED[1]*Vel_NED[1]/(Rph*cos(Pos_LLH[0])*cos(Pos_LLH[0])));
		gsl_matrix_set(EKF_F,3,2,(Vel_NED[1]*Vel_NED[1] * tan(Pos_LLH[0]) - Vel_NED[0]*Vel_NED[2])/(Rg*Rg));
		gsl_matrix_set(EKF_F,3,3,Vel_NED[2]/Rg);
		gsl_matrix_set(EKF_F,3,4,-2.0*(OMEGA_e*sin(Pos_LLH[0]) + Vel_NED[1]*tan(Pos_LLH[0])/Rg));
		gsl_matrix_set(EKF_F,3,5,Vel_NED[0]/Rg);
		gsl_matrix_set(EKF_F,4,0,2.0*OMEGA_e*(Vel_NED[0]*cos(Pos_LLH[0]) - Vel_NED[2]*sin(Pos_LLH[0])) +
								(Vel_NED[0]*Vel_NED[1]/(Rg*cos(Pos_LLH[0])*cos(Pos_LLH[0]))));
		gsl_matrix_set(EKF_F,4,2,-Vel_NED[1]*(Vel_NED[0]*tan(Pos_LLH[0]) - Vel_NED[1]*Vel_NED[2])/(Rg*Rg));
		gsl_matrix_set(EKF_F,4,3,2.0*OMEGA_e*sin(Pos_LLH[0]) + Vel_NED[1]*tan(Pos_LLH[0])/Rg);
		gsl_matrix_set(EKF_F,4,4,(Vel_NED[0]*tan(Pos_LLH[0])+Vel_NED[2])/Rg);
		gsl_matrix_set(EKF_F,4,5,2.0*OMEGA_e * cos(Pos_LLH[0]) + Vel_NED[1]/Rg);

		// velocity error due to tilt error
		gsl_matrix_set(EKF_F,3,7,-(Acc_NED[2] - LOCAL_GRAVITY));  // -fd
		gsl_matrix_set(EKF_F,3,8,(Acc_NED[1])); // fe
		gsl_matrix_set(EKF_F,4,6,(Acc_NED[2] - LOCAL_GRAVITY));  // fd
		gsl_matrix_set(EKF_F,4,8,-(Acc_NED[0])); // -fn
		gsl_matrix_set(EKF_F,5,6,-(Acc_NED[1])); // -fe
		gsl_matrix_set(EKF_F,5,7,(Acc_NED[0])); // fn

		gsl_matrix_set(EKF_F,5,0,2.0*OMEGA_e*Vel_NED[1]*sin(Pos_LLH[0]));
		gsl_matrix_set(EKF_F,5,2,(Vel_NED[0]*Vel_NED[0] + Vel_NED[1]*Vel_NED[1])/(Rg*Rg));  // note: gravity correction term missing
		gsl_matrix_set(EKF_F,5,3,-2.0*Vel_NED[0]/Rg);
		gsl_matrix_set(EKF_F,5,4,-2.0*(OMEGA_e * cos(Pos_LLH[0]) + Vel_NED[1]/Rg));
		gsl_matrix_set(EKF_F,6,0,-OMEGA_e * sin(Pos_LLH[0]));
		gsl_matrix_set(EKF_F,6,2,-Vel_NED[1]/(Rg*Rg));
		gsl_matrix_set(EKF_F,7,2,Vel_NED[0] / (Rg*Rg));
		gsl_matrix_set(EKF_F,8,0,-OMEGA_e * cos(Pos_LLH[0]) - Vel_NED[1]/(Rg*cos(Pos_LLH[0])*cos(Pos_LLH[0])));
		gsl_matrix_set(EKF_F,8,2,Vel_NED[1]*tan(Pos_LLH[0])/(Rg*Rg));

		// tilt error due to velocity error
		gsl_matrix_set(EKF_F,6,4,1.0/Rg);
		gsl_matrix_set(EKF_F,7,3,-1.0/Rg);
		gsl_matrix_set(EKF_F,8,4,-tan(Pos_LLH[0])/Rg);

		// tilt error due to inertial rotation of the reference frame
		gsl_matrix_set(EKF_F,6,7,-OMEGA_e*sin(Pos_LLH[0]) - Vel_NED[1]*tan(Pos_LLH[0])/Rg);
		gsl_matrix_set(EKF_F,6,8,Vel_NED[0]/Rg);
		gsl_matrix_set(EKF_F,7,6,OMEGA_e*sin(Pos_LLH[0]) + Vel_NED[1]*tan(Pos_LLH[0])/Rg);
		gsl_matrix_set(EKF_F,8,6,-Vel_NED[0]/Rg);
		gsl_matrix_set(EKF_F,8,7,-OMEGA_e*cos(Pos_LLH[0]) - Vel_NED[1]/Rg);
		gsl_matrix_set(EKF_F,7,8,OMEGA_e*cos(Pos_LLH[0]) + Vel_NED[1]/Rg);

		for(iRowIndex=0;iRowIndex<3;iRowIndex++)
		{
			for(iColIndex=0;iColIndex<3;iColIndex++)
			{
				gsl_matrix_set(EKF_F,6+iRowIndex,9+iColIndex,-C_BN[iRowIndex][iColIndex]);
				gsl_matrix_set(EKF_F,3+iRowIndex,12+iColIndex,C_BN[iRowIndex][iColIndex]);
			}
		}

		gsl_matrix_set(EKF_F,9,9, -x_gyro_beta);
		gsl_matrix_set(EKF_F,10,10, -y_gyro_beta);
		gsl_matrix_set(EKF_F,11,11, -z_gyro_beta);
		gsl_matrix_set(EKF_F,12,12, -x_accel_beta);
		gsl_matrix_set(EKF_F,13,13, -y_accel_beta);
		gsl_matrix_set(EKF_F,14,14, -z_accel_beta);

		/*  PHI = I + F * dt */
		gsl_matrix_set_identity(EKF_PHI);
		gsl_matrix_scale (EKF_F, 1.0/GPS_RATE);
		gsl_matrix_add (EKF_PHI, EKF_F);

//		gsl_matrix_fprintf(stdout,EKF_PHI,"%f");

		fprintf(stdout,"Q-Matrix\n");
		/* Continuous Q-Matrix */
		gsl_matrix_set_identity(EKF_Q);

		/* Discrete Qd = PHI * G * Q * G' * PHI' */
		// TODO: Qd

		fprintf(stdout,"R-Matrix\n");

		/* R - Matrix */
		gsl_matrix_set_identity(EKF_R);

		for(iIndex=0;iIndex<iNumberMeasurements;iIndex++)
		{
			gsl_matrix_set(EKF_R,iIndex,iIndex,3.0);
			gsl_matrix_set(EKF_R,iIndex+iNumberMeasurements,iIndex+iNumberMeasurements,0.3);
		}

		/* prediction step */
		/* x- = PHI * x+ */
		/* z- = H * x- */
		/* P- = PHI * P+ * PHI' + Q */
		gsl_matrix_memcpy(EKF_P_minus,EKF_Q);
		gsl_matrix_set_zero(tempP);
		gsl_blas_dgemm(CblasNoTrans,CblasNoTrans,1.0,EKF_PHI,EKF_P,1.0, tempP);
		gsl_blas_dgemm(CblasNoTrans,CblasTrans,1.0,tempP,EKF_PHI,1.0,EKF_P_minus);

		//V = H * P_minus * H' + R;
		fprintf(stdout,"HPH'+R\n");

		gsl_matrix_set_zero (tempV);
		gsl_matrix_set_zero(EKF_V);
		gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, EKF_H, EKF_P_minus, 1.0, tempV);
		gsl_matrix_memcpy(EKF_V,EKF_R);
		gsl_blas_dgemm(CblasNoTrans,CblasTrans, 1.0,tempV, EKF_H, 1.0,EKF_V);	 //this adds V = V + A*B

		/* calculate inv(EKF_V) */
		/*

		*/
		fprintf(stdout,"inv(V)\n");

		iReturnValue =  gsl_linalg_SV_decomp (EKF_V,SVD_V,SVD_S,SVDWork);
		gsl_matrix_memcpy(SVD_U,EKF_V);

		// get element wise inverse of S
		gsl_vector_set_all(SVD_Sinv,1.0);
		gsl_vector_div(SVD_Sinv, SVD_S);

		gsl_matrix_set_zero (SVD_SinvMat);
		for(iIndex=0;iIndex<2*iNumberMeasurements;iIndex++)
		{
			gsl_matrix_set(SVD_SinvMat,iIndex,iIndex,gsl_vector_get(SVD_Sinv,iIndex));
		}

		// inv(S)*UT
		gsl_matrix_set_zero (tempNN);
		gsl_blas_dgemm(CblasNoTrans,CblasTrans, 1.0, SVD_SinvMat, SVD_U, 1.0, tempNN);

		// A = V * inv(S) * UT
		gsl_matrix_set_zero(EKF_Vinv);
		gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, SVD_V, tempNN, 1.0, EKF_Vinv);

		fprintf(stdout,"PH'*inv(V)\n");

		//K = P_minus * H' * inv(V);
		gsl_matrix_set_zero (tempK);
		gsl_matrix_set_zero(EKF_K);
		gsl_blas_dgemm(CblasNoTrans,CblasTrans, 1.0, EKF_P_minus, EKF_H, 1.0, tempK);
		gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0,tempK, EKF_Vinv, 1.0,EKF_K);

		// x_hat_out = K * (z);
		gsl_vector_set_zero(EKF_x_hat_out);

		fprintf(stdout,"Kz\n");
		gsl_blas_dgemv (CblasNoTrans, 1.0, EKF_K, EKF_deltaz, 1.0, EKF_x_hat_out);

		// %symmetric form of the P-update equation to ward off divergence p347
		//%Brown and Hwang
		// P_out = (eye(NumberStates) - K*H)*P_minus*(eye(NumberStates)-K*H)' + K*R*K';

		//K*H
		gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0,EKF_K, EKF_H, 1.0,KH);

		//(eye(NumberStates) - K*H)
		gsl_matrix_set_identity(I_17by17);
		gsl_matrix_memcpy(IminusKH,I_17by17);
		gsl_matrix_sub(IminusKH, KH);

		//(eye(NumberStates) - K*H)*P_minus
		gsl_matrix_set_zero(KHPminus);
		gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, IminusKH, EKF_P_minus, 1.0, KHPminus);

		//(eye(NumberStates) - K*H)*P_minus*(eye(NumberStates)-K*H)'
		gsl_blas_dgemm(CblasNoTrans,CblasTrans, 1.0, KHPminus, IminusKH, 1.0, EKF_P);

		//K*R*K'
		gsl_matrix_set_zero (tempKRKT);
		gsl_matrix_set_zero(KRKT);
		gsl_blas_dgemm(CblasNoTrans,CblasNoTrans, 1.0, EKF_K, EKF_R, 1.0, tempKRKT);

		gsl_blas_dgemm(CblasNoTrans,CblasTrans, 1.0,tempKRKT, EKF_K, 1.0,KRKT);
		gsl_matrix_sub(EKF_P, KRKT);

		// apply correction
		Pos_LLH[0] -= gsl_vector_get(EKF_x_hat_out,0);
		Pos_LLH[1] -= gsl_vector_get(EKF_x_hat_out,1);
		Pos_LLH[2] -= gsl_vector_get(EKF_x_hat_out,2);
		Vel_NED[0] -= gsl_vector_get(EKF_x_hat_out,3);
		Vel_NED[1] -= gsl_vector_get(EKF_x_hat_out,4);
		Vel_NED[2] -= gsl_vector_get(EKF_x_hat_out,5);

		AccBias[0] -= gsl_vector_get(EKF_x_hat_out,9);
		AccBias[1] -= gsl_vector_get(EKF_x_hat_out,10);
		AccBias[2] -= gsl_vector_get(EKF_x_hat_out,11);

		GyroBias[0] -= gsl_vector_get(EKF_x_hat_out,12);
		GyroBias[1] -= gsl_vector_get(EKF_x_hat_out,13);
		GyroBias[2] -= gsl_vector_get(EKF_x_hat_out,14);

		// attitude
		del_alpha = -gsl_vector_get(EKF_x_hat_out,6);
		del_beta = -gsl_vector_get(EKF_x_hat_out,7);
		del_gamma = -gsl_vector_get(EKF_x_hat_out,8);

		del_att_skew[0][0] = 1.0;
		del_att_skew[0][1] = -del_gamma;
		del_att_skew[0][2] = del_beta;
		del_att_skew[1][0] = del_gamma;
		del_att_skew[1][1] = 1.0;
		del_att_skew[1][2] = -del_alpha;
		del_att_skew[2][0] = -del_beta;
		del_att_skew[2][1] = del_alpha;
		del_att_skew[2][2] = 1.0;

		/* C_BN = (eye(3,3) + del_att_skew) * C_BN */
		MatMul333(del_att_skew,C_BN,C_BN_new);
		memcpy(C_BN_new,C_BN,sizeof(C_BN));

		// clock
		UserClock[0] -= gsl_vector_get(EKF_x_hat_out,15);
		UserClock[1] -= gsl_vector_get(EKF_x_hat_out,16);

		/* re-enable paging */
//		munlockall();

		log("Result: \n");
		fprintf(stdout,"Pos: %f %f %f\n",
			Pos_LLH[0]*RAD2DEG,
			Pos_LLH[1]*RAD2DEG,
			Pos_LLH[2]);
		fprintf(stdout,"Vel: %f %f %f\n",
			Vel_NED[0],
			Vel_NED[1],
			Vel_NED[2]);
		fprintf(stdout,"Clock: %f %f\n",
			UserClock[0],UserClock[1]);

		fprintf(stdout,"X-out\n");
		gsl_vector_fprintf(stdout,EKF_x_hat_out,"[%f]");

//		fprintf(stdout,"delta Z\n");
//		gsl_vector_fprintf(stdout,EKF_deltaz,"[%f]");

		log("Releasing Matrices");

		gsl_vector_free(EKF_z);
		gsl_vector_free(EKF_deltaz);
		gsl_matrix_free(EKF_H);
		gsl_matrix_free(EKF_R);
		gsl_matrix_free(tempV);
		gsl_matrix_free(EKF_V);
		gsl_matrix_free(tempNN);
		gsl_vector_free(SVD_S);
		gsl_vector_free(SVD_Sinv);
		gsl_matrix_free(SVD_SinvMat);
		gsl_matrix_free(SVD_V);
		gsl_vector_free(SVDWork);
		gsl_matrix_free(EKF_K);
		gsl_matrix_free(tempK);
		gsl_matrix_free(KRKT);
		gsl_matrix_free(tempKRKT);

	}

	/* exit */
	fprintf(stdout,"%s GPS-INS Thread Exiting\n",MODULE_NAME);

	gsl_matrix_free(EKF_PHI);
	gsl_matrix_free(EKF_F);
	gsl_matrix_free(EKF_P_minus);
	gsl_matrix_free(tempP);
	gsl_matrix_free(EKF_P);
	gsl_matrix_free(I_17by17);
	gsl_vector_free(EKF_x_hat);
	gsl_matrix_free(EKF_Q);
	gsl_vector_free(temp1);
	gsl_matrix_free(KH);
	gsl_matrix_free(IminusKH);
	gsl_matrix_free(KHPminus);

#ifdef USE_RTAI
	rt_make_soft_real_time();

	if(rttGPSINS)
	{
		rt_task_delete(rttGPSINS);
	}
	stop_rt_timer();
#endif

	pthread_exit(NULL);
}


/*
ODE: a program to get optime Runge-Kutta and multi-steps methods.

Copyright 2011-2019, Javier Burguete Tolosa.

Redistribution and use in source and binary forms, with or without modification,
are permitted provided that the following conditions are met:

	1. Redistributions of source code must retain the above copyright notice,
		this list of conditions and the following disclaimer.

	2. Redistributions in binary form must reproduce the above copyright notice,
		this list of conditions and the following disclaimer in the
		documentation and/or other materials provided with the distribution.

THIS SOFTWARE IS PROVIDED BY Javier Burguete Tolosa ``AS IS'' AND ANY EXPRESS OR
IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL Javier Burguete Tolosa OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/**
 * \file rk_6_4.c
 * \brief Source file to optimize Runge-Kutta 6 steps 4th order methods.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2011-2019.
 */
#define _GNU_SOURCE
#include <string.h>
#include <math.h>
#include <libxml/parser.h>
#include <glib.h>
#include <libintl.h>
#include <gsl/gsl_rng.h>
#include "config.h"
#include "utils.h"
#include "optimize.h"
#include "rk.h"
#include "rk_6_3.h"

#define DEBUG_RK_6_3 0          ///< macro to debug.

/**
 * Function to obtain the coefficients of a 6 steps 3rd order Runge-Kutta
 * method.
 */
int
rk_tb_6_3 (Optimize * optimize) ///< Optimize struct.
{
  long double A[3], B[3], C[3], D[3];
  long double *tb, *r;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t6 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  t3 (tb) = r[3];
  b31 (tb) = r[4];
  b32 (tb) = r[5];
  t4 (tb) = r[6];
  b41 (tb) = r[7];
  b42 (tb) = r[8];
  b43 (tb) = r[9];
  t5 (tb) = r[10];
  b51 (tb) = r[11];
  b52 (tb) = r[12];
  b53 (tb) = r[13];
  b54 (tb) = r[14];
  b65 (tb) = r[15];
  b64 (tb) = r[16];
  A[0] = t1 (tb);
  B[0] = t2 (tb);
  C[0] = t3 (tb);
  D[0] = 0.5L - b64 (tb) * t4 (tb) - b65 (tb) * t5 (tb);
  A[1] = A[0] * t1 (tb);
  B[1] = B[0] * t2 (tb);
  C[1] = C[0] * t3 (tb);
  D[1] = 1.L / 3.L - b64 (tb) * sqr (t4 (tb)) - b65 (tb) * sqr (t5 (tb));
  A[2] = 0.L;
  B[2] = b21 (tb) * t1 (tb);
  C[2] = b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb);
  D[2] = 1.L / 6.L - b64 (tb) * (b41 (tb) * t1 (tb) + b42 (tb) * t2 (tb)
                                 + b43 (tb) * t3 (tb))
    - b65 (tb) * (b51 (tb) * t1 (tb) + b52 (tb) * t2 (tb)
                  + b53 (tb) * t3 (tb) + b54 (tb) * t4 (tb));
  solve_3 (A, B, C, D);
  if (isnan (D[0]) || isnan (D[1]) || isnan (D[2]))
    return 0;
  b63 (tb) = D[2];
  b62 (tb) = D[1];
  b61 (tb) = D[0];
  rk_b_6 (tb);
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 6 steps 3rd order, 4th order in
 * equations depending only in time, Runge-Kutta method.
 */
int
rk_tb_6_3t (Optimize * optimize)        ///< Optimize struct.
{
  long double A[4], B[4], C[4], D[4], E[4];
  long double *tb, *r;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3t: start\n");
#endif
  tb = optimize->coefficient;
  r = optimize->random_data;
  t6 (tb) = 1.L;
  t1 (tb) = r[0];
  t2 (tb) = r[1];
  b21 (tb) = r[2];
  t3 (tb) = r[3];
  b31 (tb) = r[4];
  b32 (tb) = r[5];
  t4 (tb) = r[6];
  b41 (tb) = r[7];
  b42 (tb) = r[8];
  b43 (tb) = r[9];
  t5 (tb) = r[10];
  b51 (tb) = r[11];
  b52 (tb) = r[12];
  b53 (tb) = r[13];
  b54 (tb) = r[14];
  b65 (tb) = r[15];
  A[0] = t1 (tb);
  B[0] = t2 (tb);
  C[0] = t3 (tb);
  D[0] = t4 (tb);
  E[0] = 0.5L - b65 (tb) * t5 (tb);
  A[1] = A[0] * t1 (tb);
  B[1] = B[0] * t2 (tb);
  C[1] = C[0] * t3 (tb);
  D[1] = D[0] * t4 (tb);
  E[1] = 1.L / 3.L - b65 (tb) * sqr (t5 (tb));
  A[2] = A[1] * t1 (tb);
  B[2] = B[1] * t2 (tb);
  C[2] = C[1] * t3 (tb);
  D[2] = D[1] * t4 (tb);
  E[2] = 0.25L - b65 (tb) * sqr (t5 (tb)) * t5 (tb);
  A[3] = 0.L;
  B[3] = b21 (tb) * t1 (tb);
  C[3] = b31 (tb) * t1 (tb) + b32 (tb) * t2 (tb);
  D[3] = b41 (tb) * t1 (tb) + b42 (tb) * t2 (tb) + b43 (tb) * t3 (tb);
  E[3] = 1.L / 6.L - b65 (tb) * (b51 (tb) * t1 (tb) + b52 (tb) * t2 (tb)
                                 + b53 (tb) * t3 (tb) + b54 (tb) * t4 (tb));
  solve_4 (A, B, C, D, E);
  if (isnan (E[0]) || isnan (E[1]) || isnan (E[2]) || isnan (E[3]))
    return 0;
  b64 (tb) = E[3];
  b63 (tb) = E[2];
  b62 (tb) = E[1];
  b61 (tb) = E[0];
  rk_b_6 (tb);
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3t: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 6 steps 2nd-3rd order Runge-Kutta
 * pair.
 */
int
rk_tb_6_3p (Optimize * optimize)        ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3p: start\n");
#endif
  if (!rk_tb_6_3 (optimize))
    return 0;
  tb = optimize->coefficient;
  e51 (tb) = 0.5L / t1 (tb);
  e52 (tb) = e53 (tb) = 0.L;
  rk_e_6 (tb);
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3p: end\n");
#endif
  return 1;
}

/**
 * Function to obtain the coefficients of a 6 steps 2nd-3rd order, 3rd-4th order
 * in equations depending only in time, Runge-Kutta pair.
 */
int
rk_tb_6_3tp (Optimize * optimize)       ///< Optimize struct.
{
  long double *tb;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3tp: start\n");
#endif
  if (!rk_tb_6_3t (optimize))
    return 0;
  tb = optimize->coefficient;
  e63 (tb) = e64 (tb) = 0.L;
  e62 (tb) = (1.L / 3.L - 0.5L * t1 (tb)) / (t2 (tb) * (t2 (tb) - t1 (tb)));
  if (isnan (e62 (tb)))
    return 0;
  e61 (tb) = (0.5L - e52 (tb) * t2 (tb)) / t1 (tb);
  if (isnan (e61 (tb)))
    return 0;
  rk_e_6 (tb);
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_tb_6_3tp: end\n");
#endif
  return 1;
}

/**
 * Function to calculate the objective function of a 6 steps 3rd order
 * Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_6_3 (RK * rk)   ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b60 (tb) < 0.L)
    o += b60 (tb);
  if (b61 (tb) < 0.L)
    o += b61 (tb);
  if (b62 (tb) < 0.L)
    o += b62 (tb);
  if (b63 (tb) < 0.L)
    o += b63 (tb);
  if (b64 (tb) < 0.L)
    o += b64 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L,
             fmaxl (t1 (tb),
                    fmaxl (t2 (tb),
                           fmaxl (t3 (tb), fmaxl (t4 (tb), t5 (tb))))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_6_3: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 6 steps 3rd order, 4th
 * order in equations depending only in time, Runge-Kutta method.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_6_3t (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3t: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b60 (tb) < 0.L)
    o += b60 (tb);
  if (b61 (tb) < 0.L)
    o += b61 (tb);
  if (b62 (tb) < 0.L)
    o += b62 (tb);
  if (b63 (tb) < 0.L)
    o += b63 (tb);
  if (b64 (tb) < 0.L)
    o += b64 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L,
             fmaxl (t1 (tb),
                    fmaxl (t2 (tb),
                           fmaxl (t3 (tb), fmaxl (t4 (tb), t5 (tb))))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3t: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_6_3t: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 6 steps 2nd-3rd order
 * Runge-Kutta pair.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_6_3p (RK * rk)  ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3p: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b60 (tb) < 0.L)
    o += b60 (tb);
  if (b61 (tb) < 0.L)
    o += b61 (tb);
  if (b62 (tb) < 0.L)
    o += b62 (tb);
  if (b63 (tb) < 0.L)
    o += b63 (tb);
  if (b64 (tb) < 0.L)
    o += b64 (tb);
  if (e60 (tb) < 0.L)
    o += e60 (tb);
  if (e61 (tb) < 0.L)
    o += e61 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L,
             fmaxl (t1 (tb),
                    fmaxl (t2 (tb),
                           fmaxl (t3 (tb), fmaxl (t4 (tb), t5 (tb))))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3p: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_6_3p: end\n");
#endif
  return o;
}

/**
 * Function to calculate the objective function of a 6 steps 2nd-3rd order,
 * 3rd-4th order in equations depending only in time, Runge-Kutta pair.
 *
 * \return objective function value.
 */
long double
rk_objective_tb_6_3tp (RK * rk) ///< RK struct.
{
  long double *tb;
  long double o;
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3tp: start\n");
#endif
  tb = rk->tb->coefficient;
  o = fminl (0.L, b20 (tb));
  if (b30 (tb) < 0.L)
    o += b30 (tb);
  if (b40 (tb) < 0.L)
    o += b40 (tb);
  if (b50 (tb) < 0.L)
    o += b50 (tb);
  if (b60 (tb) < 0.L)
    o += b60 (tb);
  if (b61 (tb) < 0.L)
    o += b61 (tb);
  if (b62 (tb) < 0.L)
    o += b62 (tb);
  if (b63 (tb) < 0.L)
    o += b63 (tb);
  if (b64 (tb) < 0.L)
    o += b64 (tb);
  if (e60 (tb) < 0.L)
    o += e60 (tb);
  if (e61 (tb) < 0.L)
    o += e61 (tb);
  if (e62 (tb) < 0.L)
    o += e62 (tb);
  if (o < 0.L)
    {
      o = 40.L - o;
      goto end;
    }
  o = 30.L
    + fmaxl (1.L,
             fmaxl (t1 (tb),
                    fmaxl (t2 (tb),
                           fmaxl (t3 (tb), fmaxl (t4 (tb), t5 (tb))))));
  if (rk->strong)
    {
      rk_bucle_ac (rk);
      o = fminl (o, *rk->ac0->optimal);
    }
end:
#if DEBUG_RK_6_3
  fprintf (stderr, "rk_objective_tb_6_3tp: optimal=%Lg\n", o);
  fprintf (stderr, "rk_objective_tb_6_3tp: end\n");
#endif
  return o;
}