/*
Ballistic: a software to benchmark ballistic models.

AUTHORS: Javier Burguete Tolosa.

Copyright 2018, AUTHORS.

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 AUTHORS ``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 AUTHORS 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 equation.c
 * \brief Source file with the equation data and functions.
 * \author Javier Burguete Tolosa.
 * \copyright Copyright 2018.
 */
#define _GNU_SOURCE
#include <stdio.h>
#include <string.h>
#include <math.h>
#include <gsl/gsl_rng.h>
#include <libxml/parser.h>
#include "config.h"
#include "utils.h"
#include "equation.h"

#define DEBUG_EQUATION 0        ///< macro to debug the equation functions.

long double r0[3];              ///< position vector.
long double r1[3];              ///< velocity vector.
long double r2[3];              ///< acceleration vector.
long double ro0[3];             ///< backup of the position vector.
long double ro1[3];             ///< backup of the velocity vector.
long double ro2[3];             ///< backup of the acceleration vector.
void (*equation_acceleration) (Equation * eq, long double *r0,
                               long double *r1, long double *r2, long double t);
///< pointer to the function to calculate the acceleration.
void (*equation_solution) (Equation * eq, long double *r0, long double *r1,
                           long double t);
///< pointer to the function to calculate the analytical solution.
long double (*equation_step_size) (Equation * eq);
///< pointer to the function to calculate the time step size.
int (*equation_land) (Equation * eq, long double, long double *t,
                      long double *dt);
///< pointer to the function to finalize the trajectory.
long double kt;
///< stability time step size coefficient.
long double dt;
///< time step size.
unsigned long int nevaluations;
///< number of evaluations of the acceleration function.

/**
 * Function to calculate the acceleration on non-resitance model.
 *
 * This function calculates the acceleration vector on a non-resistance
 * model. The movement equation is:
 * \f{equation}\ddot{\vec{r}}=\vec{g}\f}
 * with \f$\vec{g}=(0,\;0,\;-g)\f$ the gravity field vector.
 */
static void
equation_acceleration_0 (Equation * eq __attribute__ ((unused)),
                         ///< Equation struct.
                         long double *r0 __attribute__ ((unused)),
                         ///< position vector.
                         long double *r1 __attribute__ ((unused)),
                         ///< velocity vector.
                         long double *r2,       ///< acceleration vector.
                         long double t __attribute__ ((unused)))
  ///< actual time.
{
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_0: start\n");
#endif
  r2[0] = r2[1] = 0.L;
  r2[2] = -G;
  ++nevaluations;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_0: ax=%Lg ay=%Lg az=%Lg\n",
           r2[0], r2[1], r2[2]);
  fprintf (stderr, "equation_acceleration_0: end\n");
#endif
}

/**
 * Function to solve the non-resistance model.
 *
 * This function solves the movement on a resistance model characterized by the
 * movement equation:
 * \f{equation}{\ddot{\vec{r}}=\vec{g},\f}
 * with \f$\vec{g}=(0,\;0,\;-g)\f$ the gravity field vector.
 * The analytical solution is:
 * \f{equation}\dot{\vec{r}}=\dot{\vec{r}}_0+\vec{g}\,t,\f}
 * \f{equation}\vec{r}=\vec{r}_0+\dot{\vec{r}}_0\,t+\frac12\,\vec{g}\,t^2.\f}
 */
static void
equation_solution_0 (Equation * eq,     ///< Equation struct.
                     long double *r0,   ///< position vector.
                     long double *r1,   ///< velocity vector.
                     long double t)     ///< time.
{
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_0: start\n");
#endif
  r1[0] = eq->v[0];
  r1[1] = eq->v[1];
  r1[2] = eq->v[2] - eq->g * t;
  r0[0] = eq->r[0] + eq->v[0] * t;
  r0[1] = eq->r[1] + eq->v[1] * t;
  r0[2] = eq->r[2] + t * (eq->v[2] - t * 0.5L * G);
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_0: vx=%Lg vy=%Lg vz=%Lg\n",
           r1[0], r1[1], r1[2]);
  fprintf (stderr, "equation_solution_0: x=%Lg y=%Lg z=%Lg\n",
           r0[0], r0[1], r0[2]);
  fprintf (stderr, "equation_solution_0: end\n");
#endif
}

/**
 * Function to calculate the acceleration on the 1st resistance model.
 *
 * This function calculates the acceleration vector on a resistance model
 * model characterized by the movement equation:
 * \f[\ddot{\vec{r}}=\vec{g}-\lambda\,\left(\dot{\vec{r}}-\vec{w}\right)\f]
 * with \f$\vec{g}=(0,\;0,\;-g)\f$ the gravity field vector,
 * \f$\vec{w}=\left(w_x,\;w_y\;0\right)\f$ the wind velocity vector and
 * \f$\lambda\f$ a resistance coefficient.
 */
static void
equation_acceleration_1 (Equation * eq, ///< Equation struct.
                         long double *r0 __attribute__ ((unused)),
                         ///< position vector.
                         long double *r1,       ///< velocity vector.
                         long double *r2,       ///< acceleration vector.
                         long double t __attribute__ ((unused)))
  ///< actual time.
{
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_1: start\n");
#endif
  r2[0] = -eq->lambda * (r1[0] - eq->w[0]);
  r2[1] = -eq->lambda * (r1[1] - eq->w[1]);
  r2[2] = -eq->g - eq->lambda * r1[2];
  ++nevaluations;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_1: ax=%Lg ay=%Lg az=%Lg\n",
           r2[0], r2[1], r2[2]);
  fprintf (stderr, "equation_acceleration_1: end\n");
#endif
}

/**
 * Function to solve the 1st resistance model.
 *
 * This function solves the movement on a resistance model characterized by the
 * movement equation:
 * \f{equation}
 * \ddot{\vec{r}}=\vec{g}-\lambda\,\left(\dot{\vec{r}}-\vec{w}\right),
 * \f}
 * with \f$\vec{g}=(0,\;0,\;-g)\f$ the gravity field vector,
 * \f$\vec{w}=(w_x,\;w_y,\;0)\f$ the wind velocity vector and
 * \f$\lambda\f$ a resistance coefficient.
 * The analytical solution is:
 * \f{equation}
 * \dot{\vec{r}}=\dot{\vec{r}}_0\,\exp\left(-\lambda\,t\right)
 * +\left(\vec{w}+\frac{\vec{g}}{\lambda}\right)
 * \,\left[1-\exp\left(-\lambda\,t\right)\right],
 * \f}
 * \f{equation}
 * \vec{r}=\vec{r}_0+\left(\vec{w}+\frac{\vec{g}}{\lambda}\right)\,t
 * +\frac{\dot{\vec{r}}_0-\vec{w}-\vec{g}/\lambda}{\lambda}
 * \,\left[1-\exp\left(-\lambda\,t\right)\right].
 * \f}
 */
static void
equation_solution_1 (Equation * eq,     ///< Equation struct.
                     long double *r0,   ///< position vector.
                     long double *r1,   ///< velocity vector.
                     long double t)     ///< time.
{
  long double v[2];
  long double li, gl, elt, k;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_1: start\n");
#endif
  v[0] = eq->v[0] - eq->w[0];
  v[1] = eq->v[1] - eq->w[1];
  elt = expl (-eq->lambda * t);
  r1[0] = eq->w[0] + v[0] * elt;
  r1[1] = eq->w[1] + v[1] * elt;
  li = 1.L / eq->lambda;
  gl = eq->g * li;
  r1[2] = (eq->v[2] + gl) * elt - gl;
  k = li * (1.L - elt);
  r0[0] = eq->r[0] + eq->w[0] * t + v[0] * k;
  r0[1] = eq->r[1] + eq->w[1] * t + v[1] * k;
  r0[2] = eq->r[2] - gl * t + (eq->v[2] + gl) * k;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_1: vx=%Lg vy=%Lg vz=%Lg\n",
           r1[0], r1[1], r1[2]);
  fprintf (stderr, "equation_solution_1: x=%Lg y=%Lg z=%Lg\n",
           r0[0], r0[1], r0[2]);
  fprintf (stderr, "equation_solution_1: end\n");
#endif
}

/**
 * Function to calculate the acceleration on 2nd resistance model.
 *
 * This function calculates the acceleration vector on a resistance model
 * model characterized by the movement equations:
 * \f[\left.\begin{array}{r}
 * \ddot{x}=-\lambda\,\left|\dot{x}-w_x\right|\,\left(\dot{x}-w_x\right),\\
 * \ddot{y}=-\lambda\,\left|\dot{y}-w_y\right|\,\left(\dot{y}-w_y\right),\\
 * \ddot{z}=-g-\lambda\,\left|\dot{z}\right|\,\dot{z},
 * \end{array}\right\}\f]
 * with g the gravitational constant, \f$w_x\f$ and \f$w_y\f$ the wind velocity
 * vector components and \f$\lambda\f$ a resistance coefficient.
 */
static void
equation_acceleration_2 (Equation * eq, ///< Equation struct.
                         long double *r0 __attribute__ ((unused)),
                         ///< position vector.
                         long double *r1,       ///< velocity vector.
                         long double *r2,       ///< acceleration vector.
                         long double t __attribute__ ((unused)))
  ///< actual time.
{
  long double v[2];
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_2: start\n");
#endif
  v[0] = r1[0] - eq->w[0];
  v[1] = r1[1] - eq->w[1];
  r2[0] = -eq->lambda * fabsl (v[0]) * v[0];
  r2[1] = -eq->lambda * fabsl (v[1]) * v[1];
  r2[2] = -eq->g - eq->lambda * fabsl (r1[2]) * r1[2];
  ++nevaluations;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_2: ax=%Lg ay=%Lg az=%Lg\n",
           r2[0], r2[1], r2[2]);
  fprintf (stderr, "equation_acceleration_2: end\n");
#endif
}

/**
 * Function to solve the 2nd resistance mode.
 *
 * This function solves the movement on a resistance model characterized by the
 * movement equations:
 * \f{equation}\left.\begin{array}{r}
 * \ddot{x}=-\lambda\,\left|\dot{x}-w_x\right|\,\left(\dot{x}-w_x\right)\\
 * \ddot{y}=-\lambda\,\left|\dot{y}-w_y\right|\,\left(\dot{y}-w_y\right)\\
 * \ddot{z}=-g-\lambda\,\left|\dot{z}\right|\,\dot{z}
 * \end{array}\right\}\f}
 * with g the gravitational constant, \f$w_x\f$ and \f$w_y\f$ the wind velocity
 * vector components and \f$\lambda\f$ a resistance coefficient.
 * The analytical solution is:
 * \f{equation}
 * \dot{x}=w_x+\frac{\dot{x}_0-w_x}{1+\lambda\,\left|\dot{x}_0-w_x\right|\,t},
 * \f}
 * \f{equation}
 * \dot{y}=w_y+\frac{\dot{y}_0-w_y}{1+\lambda\,\left|\dot{y}_0-w_y\right|\,t},
 * \f}
 * \f{equation}
 * \dot{z}=\left\{\begin{array}{cl}
 * \dot{z}_0\leq 0\Rightarrow & \sqrt{\frac{g}{\lambda}}\,
 * \frac{\dot{z}_0\,\cosh\left(\sqrt{g\,\lambda}\,t\right)
 * -\sqrt{g/\lambda}\,\sinh\left(\sqrt{g\,\lambda}\,t\right)}
 * {\sqrt{g/\lambda}\,\cosh\left(\sqrt{g\,\lambda}\,t\right)
 * -\dot{z}_0\,\sinh\left(\sqrt{g\,\lambda}\,t\right)},\\
 * \dot{z}_0>0,\;
 * t\leq\frac{\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)}
 * {\sqrt{g\,\lambda}}\Rightarrow &
 * \sqrt{\frac{g}{\lambda}}
 * \,\tan\left[\arctan\left(\dot{z}_0\,\sqrt{\frac{\lambda}{g}}\right)
 * -\sqrt{g\,\lambda}\,t\right],\\
 * \dot{z}_0>0,\;
 * t>\frac{\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)}
 * {\sqrt{g\,\lambda}}\Rightarrow &
 * -\sqrt{\frac{g}{\lambda}}\,\tanh\left[\sqrt{g\,\lambda}\,t
 *  -\arctan\left(\dot{z}_0\,\sqrt{\frac{\lambda}{g}}\right)\right],
 * \end{array}\right.
 * \f}
 * \f{equation}
 * x=x_0+w_x\,t+\frac{\dot{x}_0-w_x}{\lambda\,\left|\dot{x}_0-w_x\right|}
 * \,\ln\left(1+\lambda\,\left|\dot{x}_0-w_x\right|\,t\right),
 * \f}
 * \f{equation}
 * y=y_0+w_y\,t+\frac{\dot{y}_0-w_y}{\lambda\,\left|\dot{y}_0-w_y\right|}
 * \,\ln\left(1+\lambda\,\left|\dot{y}_0-w_y\right|\,t\right),
 * \f}
 * \f{equation}
 * z=\left\{\begin{array}{cl}
 * \dot{z}_0\leq 0\Rightarrow & z_0
 * -\frac{\ln\left[\cosh\left(\sqrt{g\,\lambda}\,t\right)
 * -\dot{z}_0\,\sqrt{\lambda/g}\,
 * \sinh\left(\sqrt{g\,\lambda}\,t\right)\right]}{\lambda},\\
 * \dot{z}_0>0,\;
 * t\leq\frac{\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)}
 * {\sqrt{g\,\lambda}}\Rightarrow &
 * z_0+\frac{1}{\lambda}\,\ln\left\{\frac{\cos\left[
 * \arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)-\sqrt{g\,\lambda}\,t\right]}
 * {\cos\left[\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)\right]}\right\},\\
 * \dot{z}_0>0,\;
 * t>\frac{\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)}
 * {\sqrt{g\,\lambda}}\Rightarrow &
 * z_0-\frac{\ln\left\{
 * \cos\left[\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)\right]
 * \,\cosh\left[\sqrt{g\,\lambda}\,t
 * -\arctan\left(\dot{z}_0\,\sqrt{\lambda/g}\right)\right]\right\}}{\lambda}
 * \end{array}\right.
 * \f}
 */
static void
equation_solution_2 (Equation * eq,     ///< Equation struct.
                     long double *r0,   ///< position vector.
                     long double *r1,   ///< velocity vector.
                     long double t)     ///< actual time.
{
  long double v[2], k[2];
  long double tc, g_l, gl, glt, lt, li, alpha;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_2: start\n");
#endif
  v[0] = eq->v[0] - eq->w[0];
  v[1] = eq->v[1] - eq->w[1];
  lt = eq->lambda * t;
  k[0] = 1.L + lt * fabsl (v[0]);
  k[1] = 1.L + lt * fabsl (v[1]);
  r1[0] = eq->w[0] + v[0] / k[0];
  r1[1] = eq->w[1] + v[1] / k[1];
  li = 1.L / eq->lambda;
  k[0] = li * logl (k[0]);
  k[1] = li * logl (k[1]);
  r0[0] = eq->r[0] + eq->w[0] * t;
  r0[1] = eq->r[1] + eq->w[1] * t;
  if (v[0] >= 0.L)
    r0[0] += k[0];
  else
    r0[0] -= k[0];
  if (v[1] >= 0.L)
    r0[1] += k[1];
  else
    r0[1] -= k[1];
  gl = sqrtl (eq->g * eq->lambda);
  glt = gl * t;
  g_l = sqrtl (eq->g / eq->lambda);
  r0[2] = eq->r[2];
  if (eq->v[2] <= 0.L)
    {
      k[0] = coshl (glt);
      k[1] = sinhl (glt);
      r1[2] = g_l * (eq->v[2] * k[0] - g_l * k[1])
        / (g_l * k[0] - eq->v[2] * k[1]);
      r0[2] -= li * logl (k[0] - eq->v[2] * k[1] / g_l);
    }
  else
    {
      alpha = atanl (eq->v[2] / g_l);
      tc = alpha / gl;
      if (t <= tc)
        {
          r1[2] = g_l * tanl (alpha - glt);
          r0[2] += li * logl (cosl (alpha - glt) / cosl (alpha));
        }
      else
        {
          t -= tc;
          glt = gl * t;
          r1[2] = -g_l * tanhl (glt);
          r0[2] -= li * logl (cosl (alpha) * coshl (glt));
        }
    }
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_2: vx=%Lg vy=%Lg vz=%Lg\n",
           r1[0], r1[1], r1[2]);
  fprintf (stderr, "equation_solution_2: x=%Lg y=%Lg z=%Lg\n",
           r0[0], r0[1], r0[2]);
  fprintf (stderr, "equation_solution_2: end\n");
#endif
}

/**
 * Function to calculate the acceleration on a forced model.
 *
 * This function calculates the acceleration vector on a forced model. The
 * movement equation is:
 * \f{equation}
 * \ddot{\vec{r}}=\vec{g}+\vec{w}\,\exp\left(-\lambda\,t\right)
 * \f}
 * with \f$\vec{g}=(0,\;0,\;-g)\f$ the gravity field vector, \f$\lambda\f$ the
 * force decay factor and \f$\vec{w}=\left(w_x,\;w_y\;0\right)\f$ the force
 * vector.
 */
static void
equation_acceleration_3 (Equation * eq, ///< Equation struct.
                         long double *r0 __attribute__ ((unused)),
                         ///< position vector.
                         long double *r1 __attribute__ ((unused)),
                         ///< velocity vector.
                         long double *r2,       ///< acceleration vector.
                         long double t) ///< actual time.
{
  long double elt;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_3: start\n");
#endif
  elt = expl (-eq->lambda * t);
  r2[0] = eq->w[0] * elt;
  r2[1] = eq->w[1] * elt;
  r2[2] = -G;
  ++nevaluations;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_acceleration_3: ax=%Lg ay=%Lg az=%Lg\n",
           r2[0], r2[1], r2[2]);
  fprintf (stderr, "equation_acceleration_3: end\n");
#endif
}

/**
 * Function to solve the forced model.
 *
 * This function solves the movement on a forced model characterized by the
 * movement equation:
 * \f{equation}
 * \ddot{\vec{r}}=\vec{g}+\vec{w}\,\exp\left(-\lambda\,t\right)
 * \f}
 * with \f$\vec{g}=(0,\;0,\;-g)\f$ the gravity field vector, \f$\lambda\f$ the
 * force decay factor and \f$\vec{w}=\left(w_x,\;w_y\;0\right)\f$ the force
 * vector.
 * The analytical solution is:
 * \f{equation}
 * \dot{\vec{r}}=\dot{\vec{r}}_0+\vec{g}\,t
 * +\frac{\vec{w}}{\lambda}\left[1-\exp\left(-\lambda\,t\right)\right],\f}
 * \f{equation}\vec{r}=\vec{r}_0
 * +\left(\dot{\vec{r}}_0+\frac{\vec{w}}{\lambda}\right)\,t
 * +\frac12\,\vec{g}\,t^2
 * +\frac{\vec{w}}{\lambda^2}\,\left[1-\exp\left(-\lambda\,t\right)\right].
 * \f}
 */
static void
equation_solution_3 (Equation * eq,     ///< Equation struct.
                     long double *r0,   ///< position vector.
                     long double *r1,   ///< velocity vector.
                     long double t)     ///< time.
{
  long double li, k;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_3: start\n");
#endif
  li = 1.L / eq->lambda;
  k = li * (1.L - expl (-eq->lambda * t));
  r1[0] = eq->v[0] + eq->w[0] * k;
  r1[1] = eq->v[1] + eq->w[1] * k;
  r1[2] = eq->v[2] - eq->g * t;
  k *= li;
  r0[0] = eq->r[0] + (eq->v[0] + eq->w[0] * li) * t - eq->w[0] * k;
  r0[1] = eq->r[1] + (eq->v[1] + eq->w[1] * li) * t - eq->w[1] * k;
  r0[2] = eq->r[2] + t * (eq->v[2] - t * 0.5L * G);
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solution_3: vx=%Lg vy=%Lg vz=%Lg\n",
           r1[0], r1[1], r1[2]);
  fprintf (stderr, "equation_solution_3: x=%Lg y=%Lg z=%Lg\n",
           r0[0], r0[1], r0[2]);
  fprintf (stderr, "equation_solution_3: end\n");
#endif
}

/**
 * Function to solve numerically the equation by the mean point method.
 *
 * \return solution time.
 */
long double
equation_solve (Equation * eq,  ///< Equation struct.
                long double *r0,        ///< position vector solution.
                long double *r1)        ///< velocity vector solution.
{
  long double r02[3], r12[3];
  long double t1, t2, t3;
  unsigned int i;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solve: start\n");
#endif
  t1 = 0.L;
  t2 = 1.L;
  equation_solution (eq, r02, r12, t2);
  while (r02[2] > 0.L)
    {
      t2 *= 2.L;
      equation_solution (eq, r02, r12, t2);
    }
  for (i = 0; i < 64; ++i)
    {
      t3 = 0.5L * (t1 + t2);
      equation_solution (eq, r02, r12, t3);
      if (r02[2] > 0.L)
        t1 = t3;
      else
        t2 = t3;
    }
  memcpy (r0, r02, 3 * sizeof (long double));
  memcpy (r1, r12, 3 * sizeof (long double));
#if DEBUG_EQUATION
  fprintf (stderr, "equation_solve: vx=%Lg vy=%Lg vz=%Lg\n",
           r1[0], r1[1], r1[2]);
  fprintf (stderr, "equation_solve: x=%Lg y=%Lg z=%Lg\n", r0[0], r0[1], r0[2]);
  fprintf (stderr, "equation_solve: t=%Lg\n", t3);
  fprintf (stderr, "equation_solve: end\n");
#endif
  return t3;
}

/**
 * Function to set a constant time step size.
 *
 * \return time step size.
 */
static long double
equation_step_size_0 (Equation * eq __attribute__ ((unused)))
///< Equation struct.
{
#if DEBUG_EQUATION
  fprintf (stderr, "equation_step_size_0: start\n");
  fprintf (stderr, "equation_step_size_0: dt=%Lg\n", dt);
  fprintf (stderr, "equation_step_size_0: end\n");
#endif
  return dt;
}

/**
 * Function to set the time step size based on stability condition for the 1st
 * resistance model.
 *
 * \return time step size.
 */
static long double
equation_step_size_1 (Equation * eq)    ///< Equation struct.
{
  long double dt;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_step_size_1: start\n");
#endif
  dt = kt / fabsl (eq->lambda);
#if DEBUG_EQUATION
  fprintf (stderr, "equation_step_size_1: dt=%Lg\n", dt);
  fprintf (stderr, "equation_step_size_1: end\n");
#endif
  return dt;
}

/**
 * Function to set the time step size based on stability condition for the 2nd
 * resistance model.
 *
 * \return time step size.
 */
static long double
equation_step_size_2 (Equation * eq)    ///< Equation struct.
{
  long double dt;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_step_size_2: start\n");
#endif
  dt = kt / (fabsl (eq->lambda) *
             fmaxl (fabsl (r1[0] - eq->w[0]),
                    fmaxl (fabsl (r1[1] - eq->w[1]), fabsl (r1[2]))));
#if DEBUG_EQUATION
  fprintf (stderr, "equation_step_size_2: dt=%Lg\n", dt);
  fprintf (stderr, "equation_step_size_2: end\n");
#endif
  return dt;
}

/**
 * Function to finish the trajectory based on final time.
 *
 * \return 1 on finish, 0 on continuing.
 */
static int
equation_land_0 (Equation * eq, ///< Equation struct.
                 long double to,        ///< old time.
                 long double *t,        ///< next time.
                 long double *dt)       ///< time step size.
{
  long double tf;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_0: start\n");
  fprintf (stderr, "equation_land_0: to=%Lg\n", to);
#endif
  tf = eq->tf;
  if (to >= tf)
    {
#if DEBUG_EQUATION
      fprintf (stderr, "equation_land_0: landing\n");
      fprintf (stderr, "equation_land_0: end\n");
#endif
      return 1;
    }
  *t = to + *dt;
  if (*t >= tf)
    {
      *dt = tf - to;
      *t = tf;
    }
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_0: t=%Lg dt=%Lg\n", *t, *dt);
  fprintf (stderr, "equation_land_0: no landing\n");
  fprintf (stderr, "equation_land_0: end\n");
#endif
  return 0;
}

/**
 * Function to finish the trajectory based on 1st order landing.
 *
 * \return 1 on finish, 0 on continuing.
 */
static int
equation_land_1 (Equation * eq __attribute__ ((unused)),
                 ///< Equation struct.
                 long double to,        ///< old time.
                 long double *t,        ///< next time.
                 long double *dt)       ///< time step size.
{
  long double h;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_1: start\n");
  fprintf (stderr, "equation_land_1: to=%Lg\n", to);
#endif
  if (r0[2] > 0.)
    {
      *t = to + *dt;
#if DEBUG_EQUATION
      fprintf (stderr, "equation_land_1: t=%Lg dt=%Lg\n", *t, *dt);
      fprintf (stderr, "equation_land_1: no landing\n");
      fprintf (stderr, "equation_land_1: end\n");
#endif
      return 0;
    }
  h = r0[2] / r1[2];
  r0[0] -= h * r1[0];
  r0[1] -= h * r1[1];
  r0[2] -= h * r1[2];
  r1[0] -= h * r2[0];
  r1[1] -= h * r2[1];
  r1[2] -= h * r2[2];
  *t = to - h;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_1: t=%Lg dt=%Lg\n", *t, *dt);
  fprintf (stderr, "equation_land_1: landing\n");
  fprintf (stderr, "equation_land_1: end\n");
#endif
  return 1;
}

/**
 * Function to finish the trajectory based on 2nd order landing.
 *
 * \return 1 on finish, 0 on continuing.
 */
static int
equation_land_2 (Equation * eq __attribute__ ((unused)),
                 ///< Equation struct.
                 long double to,        ///< old time.
                 long double *t,        ///< next time.
                 long double *dt)       ///< time step size.
{
  long double h;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_2: start\n");
  fprintf (stderr, "equation_land_2: to=%Lg\n", to);
#endif
  if (r0[2] > 0.)
    {
      *t = to + *dt;
#if DEBUG_EQUATION
      fprintf (stderr, "equation_land_2: t=%Lg dt=%Lg\n", *t, *dt);
      fprintf (stderr, "equation_land_2: no landing\n");
      fprintf (stderr, "equation_land_2: end\n");
#endif
      return 0;
    }
  h = solve_quadratic (0.5L * r2[2], -r1[2], r0[2], 0.L, *dt);
  r0[0] -= h * (r1[0] - h * 0.5L * r2[0]);
  r0[1] -= h * (r1[1] - h * 0.5L * r2[1]);
  r0[2] -= h * (r1[2] - h * 0.5L * r2[2]);
  r1[0] -= h * r2[0];
  r1[1] -= h * r2[1];
  r1[2] -= h * r2[2];
  *t = to - h;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_2: t=%Lg dt=%Lg\n", *t, *dt);
  fprintf (stderr, "equation_land_2: landing\n");
  fprintf (stderr, "equation_land_2: end\n");
#endif
  return 1;
}

/**
 * Function to finish the trajectory based on 3rd order landing.
 *
 * \return 1 on finish, 0 on continuing.
 */
static int
equation_land_3 (Equation * eq __attribute__ ((unused)),
                 ///< Equation struct.
                 long double to,        ///< old time.
                 long double *t,        ///< next time.
                 long double *dt)       ///< time step size.
{
  long double h, r3[3];
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_3: start\n");
  fprintf (stderr, "equation_land_3: to=%Lg\n", to);
#endif
  if (r0[2] > 0.)
    {
      *t = to + *dt;
#if DEBUG_EQUATION
      fprintf (stderr, "equation_land_3: t=%Lg dt=%Lg\n", *t, *dt);
      fprintf (stderr, "equation_land_3: no landing\n");
      fprintf (stderr, "equation_land_3: end\n");
#endif
      return 0;
    }
  r3[0] = (r2[0] - ro2[0]) / *dt;
  r3[1] = (r2[1] - ro2[1]) / *dt;
  r3[2] = (r2[2] - ro2[2]) / *dt;
  h = solve_cubic (-1.L / 6.L * r3[2], 0.5L * r2[2], -r1[2], r0[2], 0.L, *dt);
  r0[0] -= h * (r1[0] - h * (0.5L * r2[0] - h * 1.L / 6.L * r3[0]));
  r0[1] -= h * (r1[1] - h * (0.5L * r2[1] - h * 1.L / 6.L * r3[1]));
  r0[2] -= h * (r1[2] - h * (0.5L * r2[2] - h * 1.L / 6.L * r3[2]));
  r1[0] -= h * (r2[0] - h * 0.5L * r3[0]);
  r1[1] -= h * (r2[1] - h * 0.5L * r3[1]);
  r1[2] -= h * (r2[2] - h * 0.5L * r3[2]);
  *t = to - h;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_land_3: t=%Lg dt=%Lg\n", *t, *dt);
  fprintf (stderr, "equation_land_3: landing\n");
  fprintf (stderr, "equation_land_3: end\n");
#endif
  return 1;
}

/**
 * Function to init the equation variables.
 */
void
equation_init (Equation * eq,   ///< Equation struct.
               gsl_rng * rng)   ///< gsl_rng struct.
{
  long double v, ha, va;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_init: start\n");
#endif
  switch (eq->type)
    {
    case 1:
    case 2:
    case 3:
      eq->lambda = eq->min_lambda
        + (eq->max_lambda - eq->min_lambda) * gsl_rng_uniform (rng);
    }
  v = eq->min_velocity
    + (eq->max_velocity - eq->min_velocity) * gsl_rng_uniform (rng);
  ha = 2.L * M_PIl * gsl_rng_uniform (rng);
  eq->r[0] = eq->r[1] = 0.L;
  va = eq->vertical_angle * M_PIl / 180.L;
  eq->v[2] = v * sinl (va);
  eq->v[1] = v * cosl (va);
  eq->v[0] = eq->v[1] * cosl (ha);
  eq->v[1] *= sinl (ha);
  v = eq->max_wind * gsl_rng_uniform (rng);
  ha = 2.L * M_PIl * gsl_rng_uniform (rng);
  eq->w[0] = v * cosl (ha);
  eq->w[1] = v * sinl (ha);
#if DEBUG_EQUATION
  fprintf (stderr, "equation_init: end\n");
#endif
}

/**
 * Function to read the equation data on a XML node.
 *
 * \return 1 on success, 0 on error.
 */
int
equation_read_xml (Equation * eq,       ///< Equation struct.
                   xmlNode * node,      ///< XML node.,
									 unsigned int initial)   ///< type of initial conditions.
{
  const char *message[] = {
    "Bad XML node",
    "Bad type",
    "Unknown type",
    "Bad x",
    "Bad y",
    "Bad z",
    "Bad vx",
    "Bad vy",
    "Bad vz",
    "Bad wx",
    "Bad wy",
    "Bad lambda",
		"Bad minimum velocity",
		"Bad maximum velocity",
		"Bad vertical angle",
		"Bad maximum wind",
		"Bad minimum lambda",
		"Bad maximum lambda",
    "Bad g",
    "Bad time step type",
    "Bad dt",
    "Bad kt",
    "Unknown time step type",
    "Bad land type",
    "Bad t",
    "Unknown land type"
  };
  int e, error_code;
#if DEBUG_EQUATION
  fprintf (stderr, "equation_read_xml: start\n");
  fprintf (stderr, "equation_read_xml: name=%s\n", node->name);
#endif
  if (xmlStrcmp (node->name, XML_EQUATION))
    {
      e = 0;
      goto exit_on_error;
    }
  eq->type = xml_node_get_uint (node, XML_TYPE, &error_code);
  if (error_code)
    {
      e = 1;
      goto exit_on_error;
    }
  switch (eq->type)
    {
    case 0:
      equation_acceleration = equation_acceleration_0;
      equation_solution = equation_solution_0;
      break;
    case 1:
      equation_acceleration = equation_acceleration_1;
      equation_solution = equation_solution_1;
      break;
    case 2:
      equation_acceleration = equation_acceleration_2;
      equation_solution = equation_solution_2;
      break;
    case 3:
      equation_acceleration = equation_acceleration_3;
      equation_solution = equation_solution_3;
      break;
    default:
      e = 2;
      goto exit_on_error;
    }
  eq->r[0] = xml_node_get_float_with_default (node, XML_X, 0.L, &error_code);
  if (error_code)
    {
      e = 3;
      goto exit_on_error;
    }
  eq->r[1] = xml_node_get_float_with_default (node, XML_Y, 0.L, &error_code);
  if (error_code)
    {
      e = 4;
      goto exit_on_error;
    }
  eq->r[2] = xml_node_get_float (node, XML_Z, &error_code);
  if (error_code || eq->r[2] < 0.L)
    {
      e = 5;
      goto exit_on_error;
    }
	if (initial)
	  {
      eq->v[0]
				= xml_node_get_float_with_default (node, XML_VX, 0.L, &error_code);
      if (error_code)
        {
          e = 6;
          goto exit_on_error;
        }
      eq->v[1]
				= xml_node_get_float_with_default (node, XML_VY, 0.L, &error_code);
      if (error_code)
        {
          e = 7;
          goto exit_on_error;
        }
      eq->v[2]
				= xml_node_get_float_with_default (node, XML_VZ, 0.L, &error_code);
      if (error_code)
        {
          e = 8;
          goto exit_on_error;
        }
      eq->w[0]
				= xml_node_get_float_with_default (node, XML_WX, 0.L, &error_code);
      if (error_code)
        {
          e = 9;
          goto exit_on_error;
        }
      eq->w[1]
			 	= xml_node_get_float_with_default (node, XML_WY, 0.L, &error_code);
      if (error_code)
        {
          e = 10;
          goto exit_on_error;
        }
      switch (eq->type)
        {
        case 1:
        case 2:
        case 3:
          eq->lambda = xml_node_get_float_with_default (node, XML_LAMBDA, 0.L,
							                                          &error_code);
          if (error_code)
            {
              e = 11;
              goto exit_on_error;
            }
				}
		}
	else
	  {
      eq->min_velocity
				= xml_node_get_float_with_default (node, XML_VMIN, 0.L, &error_code);
			if (error_code || eq->min_velocity < 0.L)
			  {
					e = 12;
					goto exit_on_error;
				}
      eq->max_velocity = xml_node_get_float (node, XML_VMAX, &error_code);
			if (error_code || eq->max_velocity <= 0.L)
			  {
					e = 13;
					goto exit_on_error;
				}
      eq->vertical_angle
				= xml_node_get_float (node, XML_VERTICAL_ANGLE, &error_code);
			if (error_code)
			  {
					e = 14;
					goto exit_on_error;
				}
      eq->max_wind
				= xml_node_get_float_with_default (node, XML_WMAX, 0.L, &error_code);
			if (error_code || eq->max_wind < 0.L)
			  {
					e = 15;
					goto exit_on_error;
				}
      switch (eq->type)
        {
        case 1:
        case 2:
        case 3:
          eq->min_lambda
						= xml_node_get_float_with_default (node, XML_LAMBDA_MIN, 0.L,
								                               &error_code);
				  if (error_code)
					  {
						  e = 16;
						  goto exit_on_error;
						}
          eq->max_lambda
						= xml_node_get_float_with_default (node, XML_LAMBDA_MAX, 0.L,
								                               &error_code);
				  if (error_code || eq->max_lambda < eq->min_lambda)
					  {
						  e = 17;
						  goto exit_on_error;
						}
			   }
		}
  eq->g = xml_node_get_float_with_default (node, XML_G, G, &error_code);
  if (error_code)
    {
      e = 18;
      goto exit_on_error;
    }
  eq->size_type = xml_node_get_uint (node, XML_TIME_STEP, &error_code);
  if (error_code)
    {
      e = 19;
      goto exit_on_error;
    }
  switch (eq->size_type)
    {
    case 0:
      equation_step_size = equation_step_size_0;
      dt = xml_node_get_float (node, XML_DT, &error_code);
      if (error_code)
        {
          e = 20;
          goto exit_on_error;
        }
      break;
    case 1:
      switch (eq->type)
        {
        case 1:
          equation_step_size = equation_step_size_1;
          break;
        case 2:
          equation_step_size = equation_step_size_2;
        }
      kt = xml_node_get_float (node, XML_KT, &error_code);
      if (error_code)
        {
          e = 21;
          goto exit_on_error;
        }
      break;
    default:
      e = 22;
      goto exit_on_error;
    }
  eq->land_type = xml_node_get_uint (node, XML_LAND, &error_code);
  if (error_code)
    {
      e = 23;
      goto exit_on_error;
    }
  switch (eq->land_type)
    {
    case 0:
      equation_land = equation_land_0;
      eq->tf = xml_node_get_float (node, XML_T, &error_code);
      if (error_code || eq->tf < 0.)
        {
          e = 24;
          goto exit_on_error;
        }
      break;
    case 1:
      equation_land = equation_land_1;
      break;
    case 2:
      equation_land = equation_land_2;
      break;
    case 3:
      equation_land = equation_land_3;
      break;
    default:
      e = 25;
      goto exit_on_error;
    }
#if DEBUG_EQUATION
  fprintf (stderr, "equation_read_xml: success\n");
  fprintf (stderr, "equation_read_xml: end\n");
#endif
  return 1;
exit_on_error:
#if DEBUG_EQUATION
  fprintf (stderr, "equation_read_xml: error\n");
#endif
  error_add (message[e]);
#if DEBUG_EQUATION
  fprintf (stderr, "equation_read_xml: end\n");
#endif
  return 0;
}


#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>
#include <assert.h>
#include <alloca.h>

#include <gsl/gsl_cdf.h>

#include "limits.h"
#include "stattest.h"
#include "cat.h"
#include "fisher.h"
#include "min2.h"
#include "bvr.h"

typedef unsigned int count_t;
typedef const count_t COUNT_T;
typedef double       prob_t;

/**
  * This class is stateful in the interest of efficiency
  * because some methods share preliminary computations:
  * 1) statistics require expectation, and
  * 2) expectation requires marginals
  * (which may also be used incidentally for other purposes), so this
  * variable captures the progress towards complete calculations
  * to enable implicit just-in-time calculation.
  */
enum State {
	Nothing = 0,
	Marginals,
	Expectation
};

struct CatCovars {

	/**
	 * Buffers are sized to accomodate these dimensions at construction
	 * and are not changeable thereafter.
	 */
	unsigned int ROW_CAPACITY, COL_CAPACITY;

	/**
	  * "decl(ared)_" because the bounds says nothing about the row/col
	  * content--empty rows/columns may exist! I'm just driving this point
	  * home in these members' names because it has already burned me once!
	  */
	unsigned int decl_rows, decl_cols, decl_cells;

	/**
	  * Count of samples pushed since last cat_clear.
	  */
	count_t sample_count;

	// Calculation state ///////////////////////////////////////////////////

	prob_t minimumExpected;

	/**
	 * This is used strictly by badCellExists
	 */
	count_t REQUIRED_CELL_MINIMUM;

	enum State calculated;

	// Buffers /////////////////////////////////////////////////////////////

	size_t SIZEOF_PROB_BUF;

	prob_t  *expect;

	size_t SIZEOF_COUNT_BUF;

	/**
	  * The counts matrix is stored packed against the head of this
	  * (heap-allocated) buffer. In other words, though this buffer
	  * is -expected- to be larger than required (since the class
	  * is designed to be iteratively re-used), only the first
	  * (decl_rows*decl_cols) of the buffer are used for any given
	  * matrix.
	  */
	count_t *counts;

	/**
	  * Following are derivative data...computed from counts.
	  */
	count_t *rmarg;
	count_t *cmarg;
};

static void _coordsOfOffset(
	struct CatCovars *co, unsigned int off, unsigned int *r, unsigned int *c ) {
	*r = off / co->decl_cols;
	*c = off % co->decl_cols;
}

static count_t _count( struct CatCovars *co, coord_t r, coord_t c ) {
	const int C = co->decl_cols;
	assert( r < co->decl_rows && c < C );
	return co->counts[ r*C + c ];
}

static prob_t _expected( struct CatCovars *co, coord_t r, coord_t c ) {
	const int C = co->decl_cols;
	assert( r < co->decl_rows && c < C );
	return co->expect[ r*C + c ];
}

static void _recalcSampleCount( struct CatCovars *co ) {
	int n = co->decl_cells;
	co->sample_count = 0;
	while( n-- > 0 ) co->sample_count += co->counts[n];
}

/**
  * This starts searching at the given coordinates and continues linearly
  * through the matrix until it finds a violating cell at which point it
  * turns the linear index into 2D coordinates.
  */
static bool _badCellExists( struct CatCovars *co,
		unsigned int *offset ) {

	unsigned int i = *offset;
	while( i < co->decl_cells ) {
		if( co->counts[i] < co->REQUIRED_CELL_MINIMUM ) {
			*offset = i;
			return true;
		}
		i++;
	}
	return false;
}

/**
  * If after accumulation there are fewer than 2 rows or fewer than 2
  * columns, the table is degenerate. Rows or columns with zero marginal
  * sum are OK! For a contingency table that's bad but not -degenerate-.
  */
static bool _immediatelyDegenerate( struct CatCovars *co ) {
	return ! ( co->decl_rows >= 2 && co->decl_cols >= 2 );
}

#if 0
static bool _eventuallyDegenerate( struct CatCovars *co ) {
	unsigned nonemprows = 0;
	unsigned nonempcols = 0;
	unsigned i;
	for(i = 0; i < co->decl_rows; i++ ) if( co->rmarg[i] > 0 ) nonemprows++;
	for(i = 0; i < co->decl_cols; i++ ) if( co->cmarg[i] > 0 ) nonempcols++;
	return  !  ( nonemprows >= 2 && nonempcols >= 2 );
}
#endif

static void _cullRow( struct CatCovars *co, unsigned int r ) {

	// An over-writing copy suffices since matrix is
	// stored row-major. mergeCols is more intricate.

	if( r+1 < co->decl_rows ) { // ...so 0 < rows-U-1
		memmove(
			co->counts + (r  )*co->decl_cols,
			co->counts + (r+1)*co->decl_cols,
			(co->decl_rows - r - 1)*co->decl_cols*sizeof(count_t) );
	} else {
		// ...it's the last row, and there's nothing to move.
	}
	co->decl_rows  -= 1;
	co->decl_cells -= co->decl_cols;

	co->sample_count = 0;
	co->calculated = Nothing; // ...to trigger recomputation.
}

static void _cullCol( struct CatCovars *co, unsigned int c ) {

	const unsigned int N = co->decl_cells;
	// Shrink. Because matrices are stored row-major and calculations
	// throughout this class assume the matrix is packed at the head of
	// the buffer, there is actually a fairly complex shifting of all
	// but the first c entries that has to occurr...
	// Basically, traverse the matrix linearly (storage order) shifting
	// "left" all entries that are NOT in the c'th column...
	for(unsigned int src = 0, dst = 0 ; src < N; src++ ) {
		if( src % co->decl_cols != c ) {
			co->counts[dst++] = co->counts[src];
		}
	}
	co->decl_cols -= 1;
	co->decl_cells -= co->decl_rows;

	co->sample_count = 0;
	co->calculated = Nothing; // ...to trigger recomputation.
}

/**
  * Following two MUST be called in order before any of
  * the statistics are computed. In other words,
  *    !!!! THESE ARE NOT CALLED IMPLICITLY! !!!!
  * (Statistical methods which -ought- to be const can't be unless
  *  'expect', '[rc]marg' and friends are made mutable->)
  */
static void _calc_marginals( struct CatCovars *co ) {

	const int R = co->decl_rows;
	const int C = co->decl_cols;
	co->sample_count = 0;

	memset( co->rmarg, 0, R*sizeof(count_t) );
	memset( co->cmarg, 0, C*sizeof(count_t) );

	// Calculate the marginals...

	for(unsigned int i = 0; i < R; i++ ) {
		for(unsigned int j = 0; j < C; j++ ) {
			const count_t C = _count( co, i,j);
			co->rmarg[i] += C;
			co->cmarg[j] += C;
			co->sample_count    += C;
		}
	}

	co->calculated = Marginals;
}

/**
 * Calculates the expected probabilities of a contingency table assuming
 * independence of the two variables.
 * "Since the Chi-square and G^2 statistics rest on the normal approximation
 *  to the hypergeometric distribution, these approximations get strained
 *  when there are zero frequency cells or when the expected counts are
 *  'very small'." ...from Yaramakala's Masters Thesis, p.43
 *
 * "...for an independence test to be reliable, all cells have non-zero
 *  expected values and at least 80% of the cells should have expected
 *  values greater than 5." [Everitt 1977, Upton 1978]
 */
static void _calc_expectation( struct CatCovars *co ) {

	const int R = co->decl_rows;
	const int C = co->decl_cols;

	if( co->calculated < Marginals )
		_calc_marginals( co );

	if( co->sample_count > 0 ) {
		co->minimumExpected
			= (prob_t)(co->rmarg[0] * co->cmarg[0])
			/ (prob_t)co->sample_count;
		for(unsigned int i = 0; i < R; i++ ) {
			for(unsigned int j = 0; j < C; j++ ) {
				const prob_t E
					= (prob_t)(co->rmarg[i] * co->cmarg[j])
					/ (prob_t)co->sample_count;
				co->expect[i*co->decl_cols + j] = E;
				if( co->minimumExpected > E )
					co->minimumExpected = E;
			}
		}
	}

	co->calculated = Expectation;
}

/**
 * This function cannot fail. Valid results always exist.
 * Returns the indices of the minimal row and columns marginals.
 */
#if 0
static void _minimalMarginals( struct CatCovars *co, unsigned int *rm, unsigned int *cm ) {

	unsigned int MIN_DIM
		= co->decl_rows < co->decl_cols ? co->decl_rows : co->decl_cols;
	unsigned int i, ri = 0, ci = 0;
	count_t mr, mc;

	if( co->calculated < Marginals )
		_calc_marginals( co );

	mr = co->rmarg[ri];
	mc = co->cmarg[ci];

	// Iterate over marginals at indices [0,min(rows,cols) )

	for(i = 0; i < MIN_DIM; i++ ) {
		if( mr > co->rmarg[i] ) {
			mr = co->rmarg[i];
			ri = i;
		}
		if( mc > co->cmarg[i] ) {
			mc = co->cmarg[i];
			ci = i;
		}
	}

	// ...continue iteration over [ min(rows,cols), max(rows,cols) ).

	if( co->decl_cols > co->decl_rows )
		for(; i < co->decl_cols; i++ ) {
			if( mc > co->cmarg[i] ) {
				mc = co->cmarg[i];
				ci = i;
			}
		}
	else
	if( co->decl_rows > co->decl_cols )
		for(; i < co->decl_rows; i++ ) {
			if( mr > co->rmarg[i] ) {
				mr = co->rmarg[i];
				ri = i;
			}
		}

	*rm = ri;
	*cm = ci;
}
#endif

/**
 * A pretty printer...primarily for debugging.
 */
#ifdef _DEBUG
static void dbg_dump( struct CatCovars *co, FILE *fp, const char *prefix ) {

	if( prefix==NULL ) prefix="";

	fprintf( fp, "%s %d(r) x %d(c) (capacity = %dx%d)\n",
		prefix,
		co->decl_rows,
		co->decl_cols,
		co->ROW_CAPACITY,
		co->COL_CAPACITY );

	fprintf( fp, "%s cell counts...\n", prefix );
	for(unsigned int r = 0; r < co->decl_rows; r++ ) {
		fputs( prefix, fp );
		for(unsigned int c = 0; c < co->decl_cols; c++ ) {
			fprintf( fp, "\t%d", _count(co,r,c) );
		}
		fputc( '\n', fp );
	}

	if( co->calculated >= Expectation ) {
		fprintf( fp, "%s %d total samples, expectation...\n", prefix, co->sample_count );
		for(unsigned int r = 0; r < co->decl_rows; r++ ) {
			fputs( prefix, fp );
			for(unsigned int c = 0; c < co->decl_cols; c++ ) {
				fprintf( fp, "\t%.3e", _expected( co, r, c) );
			}
			fputc( '\n', fp );
		}
	}
}
#endif

/***************************************************************************
  * Publics
  */

void cat_destroy( void *pv ) {

	if( pv ) {
		struct CatCovars *co = (struct CatCovars *)pv;
		if( co->expect )
			free( co->expect );
		free( pv );
	}
}

/**
  * Pre-allocate a set of working buffers large enough for all anticipated
  * calculations (max feature length) and a struct to wrap them.
  */
void *cat_create( unsigned int rcap, unsigned int ccap ) {

	struct CatCovars *co
		= calloc( 1, sizeof(struct CatCovars) );

	if( co ) {
		co->ROW_CAPACITY = rcap;
		co->COL_CAPACITY = ccap;
		co->SIZEOF_COUNT_BUF
			= (rcap*ccap+rcap+ccap)*sizeof(count_t);
		co->SIZEOF_PROB_BUF
			= rcap*ccap*sizeof(prob_t);

		// Allocate one large buffer and partition it up.
		co->expect
			= calloc( co->SIZEOF_PROB_BUF + co->SIZEOF_COUNT_BUF, sizeof(char) );
		co->counts
			= (count_t*)( co->expect + rcap*ccap );
		co->rmarg
			= co->counts + rcap*ccap;
		co->cmarg
			= co->rmarg + rcap;

		co->REQUIRED_CELL_MINIMUM = 5;

		// If -anything- failed clean up any successes.
		if( (NULL == co->counts) ||
			(NULL == co->expect) ) {
			cat_destroy( co );
			return NULL;
		}
		return co;
	}
	return NULL;
}

void cat_clear( void *pv, coord_t nr, coord_t nc ) {

	struct CatCovars *co = (struct CatCovars *)pv;

	assert( nr <= co->ROW_CAPACITY && nc <= co->COL_CAPACITY );

	// Set dimensions and cell count.

	co->decl_rows  = nr;
	co->decl_cols  = nc;
	co->decl_cells = nr*nc;

	co->sample_count    = 0;
	co->minimumExpected = 0.0;
	co->calculated = Marginals;

	// OPTIMIZATION: Only clearing the part of capacity intended for use!

	memset( co->counts, 0, co->decl_cells*sizeof(count_t) );
	memset( co->rmarg,  0, co->decl_rows*sizeof(count_t) );
	memset( co->cmarg,  0, co->decl_cols*sizeof(count_t) );
	memset( co->expect, 0, co->decl_cells*sizeof(prob_t) );
}

void cat_setMinCellCount( void *pv, unsigned n ) {
	struct CatCovars *co = (struct CatCovars *)pv;
	co->REQUIRED_CELL_MINIMUM = n;
}

/**
 * Only this one is public in order that we may count the samples
 * as they're added...
 */
void cat_push( void *pv, coord_t r, coord_t c ) {

	struct CatCovars *co = (struct CatCovars *)pv;

	assert( r < co->decl_rows );
	assert( c < co->decl_cols );

	co->counts[ r*co->decl_cols + c ] += 1;

	// size and marginals are kept up to date...
	co->sample_count++;
	co->rmarg[r] += 1;
	co->cmarg[c] += 1;
}

size_t cat_size( void *pv ) {
	struct CatCovars *co = (struct CatCovars *)pv;
	return co->sample_count;
}

bool cat_complete( void *pv ) {
	struct CatCovars *co = (struct CatCovars *)pv;
	return ! _immediatelyDegenerate( co );
}

bool cat_is2x2( void *pv ) {
	struct CatCovars *co = (struct CatCovars *)pv;
	return (2 == co->decl_rows) && (2 == co->decl_cols);
}

/**
  * <bits> will ALWAYS be non-zero on entry, but it may only contain
  * a single set bit.
  */
static int _offsetOfLeastLoss( unsigned bits, COUNT_T *loss, count_t *leastLoss ) {
	int v = __builtin_ctz( bits );
	int l = loss[v];
	bits ^= (1<<v);
	while( bits ) {
		const int i = __builtin_ctz( bits );
		bits ^= (1<<i);
		if( l > loss[i] ) {
			l = loss[i];
			v = i;
		}
	}
	*leastLoss = l;
	return v;
}

/**
 * This removes all rows and any columns containing cells with
 * counts LESS THAN OR EQUAL TO REQUIRED_CELL_MINIMUM.
 *
 * This method assumes we start with a table larger in at least
 * one dimention than 2x2 and halts when/if we reach a 2x2.
 */
unsigned int cat_cullBadCells( void *pv, char *log, int buflen ) {

	unsigned int culled = 0;
	struct CatCovars *co = (struct CatCovars *)pv;
	const char * const EOL = log + buflen - 2; // leave room for "+\0".
	// BEGIN log maintenance...
	const int MIN_LOG_RECORD_LEN = 3; // "R99"
	// ...since tables will never exceed 99 rows or columns.
	char *pc = log;
	// ...END log maintenance.
	unsigned int victim;
	unsigned int raw_mem_offset = 0; // linear offsets of "bad" cell.

#ifdef _DEBUG
	const bool SHOW_CULLING = getenv("SHOW_CULLING") != NULL;
	if( SHOW_CULLING ) {
		fprintf( stderr, "CULL log: original matrix\n" );
		dbg_dump( co, stderr, "CULL" );
	}
#endif

	assert( ! _immediatelyDegenerate( co ) ); // so BOTH dims >= 2

	while( (co->decl_rows > 2 || co->decl_cols > 2) && _badCellExists( co, &raw_mem_offset ) ) {

		unsigned int r, c;
		char cullty = '?';

		/**
		  * It is always necessary to cull a row or column.
		  * If |columns| > 3, calculate "best" column to cull.
		  * If |rows| > 3, calculate "best" row to cull.
		  * "Best" victim is row OR column among the candidates for
		  * culling with minimal marginal.
		  */

		unsigned row_bitfield = 0;
		unsigned col_bitfield = 0;
		count_t minRowCost, minColCost;

		assert( sizeof(row_bitfield)*8 >= MAX_CATEGORY_COUNT );

		// Find ALL rows and columns that are candidates for culling by
		// virtue of containing a bad cell.

		do {
			_coordsOfOffset( co, raw_mem_offset, &r, &c );
			row_bitfield |= (1<<r);
			col_bitfield |= (1<<c);
			++ raw_mem_offset;
			// Add the row and column to the list of candidate victims.
		} while( _badCellExists( co, &raw_mem_offset ) );

		if( co->calculated < Marginals )
			_calc_marginals( co );

		if( co->decl_rows > 2 ) {

			r = _offsetOfLeastLoss( row_bitfield, co->rmarg, &minRowCost );

			if( co->decl_cols > 2 ) { // EITHER a col OR row may be culled

				c = _offsetOfLeastLoss( col_bitfield, co->cmarg, &minColCost );
				cullty = minRowCost < minColCost ? 'R' : 'C';

			} else                    // ONLY a row may be culled
				cullty = 'R';

		} else {                      // ONLY a column may be culled

			c = _offsetOfLeastLoss( col_bitfield, co->cmarg, &minColCost );
			cullty = 'C';
		}

		if( cullty == 'R' )
			_cullRow( co, (victim = r) );
		else {
			assert( cullty == 'C' );
			_cullCol( co, (victim = c) );
		}

		// Log it, if possible...

		if( pc ) {

			pc += sprintf( pc, "%c%d", cullty, victim );

			// Following is a bit of overkill to make sure we can
			// give some indication that we ran out of log space.

			if( EOL - pc < MIN_LOG_RECORD_LEN ) {
				if( EOL - pc > 0 ) strcpy( pc, "+");
				pc = NULL; // no more logging.
				// Really this should never be executed if log buffer
				// is always sufficient.
			}
		}

		// Unlike merging, decimating CAN leave a 0 cell at a lower
		// linear index, so reset is required...
		culled++;
		raw_mem_offset = 0;

#ifdef _DEBUG
		if( SHOW_CULLING ) {
			fprintf( stderr,
				"CULL After %d selected %c%d (from R:%08X,C:%08X) costing %d samples\n",
				culled, cullty, victim,
				row_bitfield, col_bitfield,
			   	cullty=='R' ? minRowCost : minColCost );
			dbg_dump( co, stderr, "CULL" );
		}
#endif
	}

	if( culled > 0 )
		_recalcSampleCount( co );

	return culled;
}

#ifdef HAVE_G_TEST
int cat_g( void *pv, struct Statistic *result ) {

	struct CatCovars *co = (struct CatCovars *)pv;
	const int R = co->decl_rows;
	const int C = co->decl_cols;
	double g = 0.0;

	abort(); // unfinished.

	if( co->calculated < Expectation )
		_calc_expectation( co );

	for(unsigned int i = 0; i < R; i++ ) {
		for(unsigned int j = 0; j < C; j++ ) {
			const double O = _count( co, i, j );
			const double E = _expected( co, i, j );
			if( E > 0 && O > 0 )
				g += ( O * log( O / E ) );
		}
	}

	g *= 2.0; // TODO: verify!

	result->name
		= "G";
	result->sample_count
		= co->sample_count;
	result->probability
		= gsl_cdf_chisq_Q( g, (R-1)*(C-1) );

	return 0;
}
#endif

int cat_chi_square( void *pv, struct Statistic *result ) {

	struct CatCovars *co = (struct CatCovars *)pv;
	const int R = co->decl_rows;
	const int C = co->decl_cols;

	unsigned int n_empty = 0;
	double chi = 0.0;

	if( co->calculated < Expectation )
		_calc_expectation( co );

	for(unsigned int i = 0; i < R; i++ ) {
		for(unsigned int j = 0; j < C; j++ ) {
			const double O = _count( co, i, j );
			const double E = _expected( co, i, j );
			if( E > 0 && O > 0 )
				chi += ((O-E)*(O-E))/E;
			else
			if( O == 0 )
				n_empty++;
		}
	}

	result->name
		= "Chi-square";
	result->sample_count
		= co->sample_count;
	result->probability
		= gsl_cdf_chisq_Q( chi, (R-1)*(C-1) );

	result->extra_value[0] = R;
	result->extra_value[1] = C;
	result->extra_value[2] = co->minimumExpected;
	result->extra_value[3] = n_empty;

	return 0;
}

int cat_fisher_exact( void *pv, struct Statistic *result ) {

	struct CatCovars *co = (struct CatCovars *)pv;

	// OPTIMIZE: does not make sense to call fisher exact without
	// NULL stest_t argument. Keeping this signature for consistency
	// with all other stats, but it might make sense to change the
	// signature of just this function to return P-value (unless I
	// ever added log-likelihood ratio calculation which is what
	// ought to be returned as the function value).

	/**
	  * Given the table:
	  *    a | b
	  *    --+--
	  *    c | d
	  *
	  * ...we need to pass fexact_prob( x, m, n, k )
	  * m == a+c
	  * n == b+d
	  * k == a+b
	  * x == a
	  */
	result->name
		= "Fisher_Exact";
	result->sample_count
		= co->sample_count;
	result->probability
		= fexact_prob(
			_count(co,0,0),
			_count(co,0,0) + _count(co,1,0),   // a+c
			_count(co,0,1) + _count(co,1,1),   // b+d
			_count(co,0,0) + _count(co,0,1) ); // a+b

	// TODO: Don't need these now.
	result->extra_value[0] = co->decl_rows;
	result->extra_value[1] = co->decl_cols;

	return 0;
}

/**
  */
double cat_spearman_rho( void *pv ) {

	struct CatCovars *co = (struct CatCovars *)pv;
	double rho = nan("nan");
	const unsigned r0 = _count(co,0,0) + _count(co,0,1);
	const unsigned r1 = _count(co,1,0) + _count(co,1,1);
	const unsigned c0 = _count(co,0,0) + _count(co,1,0);
	const unsigned c1 = _count(co,0,1) + _count(co,1,1);

	// Continuing is pointless if any of these are 0.

	if( r0 > 0 && r1 > 0 && c0 > 0 && c1 > 0 ) {

		const float MU    = (co->sample_count+1.0)/2.0;
		const float r0_mu = (r0+1.0)/2.0;
		const float r1_mu = MEAN_RANK_OF_TIES( r0, co->sample_count );
		const float c0_mu = (c0+1.0)/2.0;
		const float c1_mu = MEAN_RANK_OF_TIES( c0, co->sample_count );

		/**
		  * Following is a heavily compacted form of the Pearson rho,
		  * compacted because thanks to all the ties there are only two
		  * "versions" of (x_i-x_mu) and (y_i-x_mu), and the general
		  * sums in the Pearson quotient just become multiples of these.
		  */
		const double DENOM
			= sqrt(
			  (r0*(r0_mu-MU)*(r0_mu-MU) + r1*(r1_mu-MU)*(r1_mu-MU))
			* (c0*(c0_mu-MU)*(c0_mu-MU) + c1*(c1_mu-MU)*(c1_mu-MU)) );

		if( isnormal(DENOM) ) {
			rho =
				( _count(co,0,0)*(r0_mu-MU)*(c0_mu-MU)
				+ _count(co,0,1)*(r0_mu-MU)*(c1_mu-MU)
				+ _count(co,1,0)*(r1_mu-MU)*(c0_mu-MU)
				+ _count(co,1,1)*(r1_mu-MU)*(c1_mu-MU) )
				/
				DENOM;
		}

	}
	return rho;
}

#ifdef _UNITTEST_CAT_

#include <err.h>

int main( int argc, char *argv[] ) {

	if( argc >= 3 ) {

		const int MERGE_LOG_LEN = 128;
		char merge_log[ MERGE_LOG_LEN ];

		struct Statistic result;

		const unsigned int MAXROW
			= atoi(argv[1]);
		const unsigned int MAXCOL
			= atoi(argv[2]);

		FILE *fp
			= argc > 3
			? fopen( argv[3], "r" )
			: stdin;

		int err = 0;
		char *line = NULL;
		size_t n = 0;

		void *accum
			= cat_create( MAXROW, MAXCOL);

		cat_setMinCellCount( accum, 5 );
		cat_clear( accum, MAXROW, MAXCOL );   // ...as per command line.

		while( getline( &line, &n, fp ) > 0 ) {
			int cat[2];
			if( line[0] == '#' ) {
				fputs( line, stdout );
				continue;
			}
			if( 2 == sscanf( line, "%d\t%d\n", cat+0, cat+1 ) ) {
				if( 0 <= cat[0] && 0 <= cat[1] ) {
					cat_push( accum, cat[0], cat[1] );
				}
			} else {
				fprintf( stderr, "Failure parsing line: %s", line );
			}
		}
		free( line );
		fclose( fp );

		puts( "Original..." );
		dbg_dump( accum, stdout, ""  );

		if( cat_cullBadCells( accum, merge_log, MERGE_LOG_LEN ) > 0 ) {
			fprintf( stdout, "Culled %s, leaving...\n", merge_log );
			dbg_dump( accum, stdout, "" );
		}

		memset( &result, 0, sizeof(struct Statistic) );
		err = cat_is2x2( accum )
			? cat_fisher_exact( accum, &result )
			: cat_chi_square( accum, &result );

		if( ! err ) {
			printf( "p-value=%f", result.probability );
			if( cat_is2x2( accum ) )
				printf( ", spearman=%f\n", cat_spearman_rho( accum ) );
			else
				fputc( '\n', stdout );
		} else
			printf( "error\n" );

		cat_destroy( accum );

	} else
		err( -1, "%s <max row> <max col> [ <2-column file> ]", argv[0] );
	return 0;
}
#endif


/**
 * \author Sylvain Marsat, University of Maryland - NASA GSFC
 *
 * \brief C header for functions windowing and computing FFT/IFFT of time/frequency series.
 *
 */

#ifndef __COMPUTELISASNR_H__
#define __COMPUTELISASNR_H__ 1

#ifdef __GNUC__
#define UNUSED __attribute__ ((unused))
#else
#define UNUSED
#endif

#define _XOPEN_SOURCE 500

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <complex.h>
#include <time.h>
#include <unistd.h>
#include <getopt.h>
#include <stdbool.h>
#include <string.h>

#include <gsl/gsl_errno.h>
#include <gsl/gsl_bspline.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_min.h>
#include <gsl/gsl_spline.h>
#include <gsl/gsl_complex.h>

#include "constants.h"
#include "struct.h"
#include "waveform.h"
#include "fft.h"
#include "LISAgeometry.h"
#include "LISAFDresponse.h"
#include "LISAutils.h"

/********************************** Structures ******************************************/

/* Parameters for the generation of a LISA waveform (in the form of a list of modes) */
typedef struct tagComputeLISASNRparams {
  double tRef;               /* reference time (s) - GPS time at the frequency representing coalescence */
  double phiRef;             /* reference phase (rad) - phase at the frequency representing coalescence (or at fRef if specified) */
  double fRef;               /* reference frequency at which phiRef is set (Hz, default 0 which is interpreted as Mf=0.14) */
  double m1;                 /* mass of companion 1 (solar masses, default 2e6) */
  double m2;                 /* mass of companion 2 (solar masses, default 1e6) */
  double distance;           /* distance of source (Mpc, default 1e3) */
  double inclination;        /* inclination of source (rad, default pi/3) */
  double lambda;             /* first angle for the position in the sky (rad, default 0) */
  double beta;               /* second angle for the position in the sky (rad, default 0) */
  double polarization;       /* polarization angle (rad, default 0) */

  int nbmode;                /* number of modes to generate (starting with 22) - defaults to 5 (all modes) */
  double deltatobs;          /* Observation duration (years, default=2) */
  double minf;               /* Minimal frequency (Hz, default=0) - when set to 0, use the lowest frequency where the detector noise model is trusted __LISASimFD_Noise_fLow (set somewhat arbitrarily)*/
  double maxf;               /* Maximal frequency (Hz, default=0) - when set to 0, use the highest frequency where the detector noise model is trusted __LISASimFD_Noise_fHigh (set somewhat arbitrarily)*/
  int tagextpn;              /* Tag to allow PN extension of the waveform at low frequencies */
  double Mfmatch;            /* When PN extension allowed, geometric matching frequency: will use ROM above this value. If <=0, use ROM down to the lowest covered frequency */
  int tagtdi;                /* Tag selecting the desired TDI observables */
  int tagint;                /* Tag choosing the integrator: 0 for Fresnel (default), 1 for linear integration */
  int nbptsoverlap;          /* Number of points to use in loglinear overlaps (default 32768) */
  LISAconstellation *variant;  /* A structure defining the LISA constellation features */
  int frozenLISA;            /* Freeze the orbital configuration to the time of peak of the injection (default 0) */
  ResponseApproxtag responseapprox;    /* Approximation in the GAB and orb response - choices are full (full response, default), lowfL (keep orbital delay frequency-dependence but simplify constellation response) and lowf (simplify constellation and orbital response) - WARNING : at the moment noises are not consistent, and TDI combinations from the GAB are unchanged */
  int fromtditdfile;         /* Option for loading time series for TDI observables and FFTing */
  int nlinesinfile;          /* Number of lines of input file */
  char indir[256];           /* Input directory */
  char infile[256];          /* Input file */
  int loadparamsfile;        /* Option to load physical parameters from file and to output result to file (default 0) */
  int nlinesparams;          /* Number of lines in params file */
  char paramsdir[256];       /* Directory for the input/output file */
  char paramsfile[256];      /* Input file with the parameters */
  char outputfile[256];      /* Output file */
} ComputeLISASNRparams;

#if 0
{ /* so that editors will match succeeding brace */
#elif defined(__cplusplus)
}
#endif

#endif /* _COMPUTELISASNR_H */


/* multifit/fdfridge.c
 *
 * Copyright (C) 2014 Patrick Alken
 *
 * 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 3 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#include <config.h>
#include <stdlib.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multifit_nlin.h>
#include <gsl/gsl_blas.h>

static int fdfridge_f(const gsl_vector * x, void * params, gsl_vector * f);
static int fdfridge_df(const gsl_vector * x, void * params, gsl_matrix * J);

gsl_multifit_fdfridge *
gsl_multifit_fdfridge_alloc (const gsl_multifit_fdfsolver_type * T,
                             const size_t n, const size_t p)
{
  gsl_multifit_fdfridge * work;

  work = calloc(1, sizeof(gsl_multifit_fdfridge));
  if (work == NULL)
    {
      GSL_ERROR_VAL("failed to allocate workspace",
                    GSL_ENOMEM, 0);
    }

  work->s = gsl_multifit_fdfsolver_alloc (T, n + p, p);
  if (work->s == NULL)
    {
      gsl_multifit_fdfridge_free(work);
      GSL_ERROR_VAL("failed to allocate space for fdfsolver",
                    GSL_ENOMEM, 0);
    }

  work->wts = gsl_vector_alloc(n + p);
  if (work->wts == NULL)
    {
      gsl_multifit_fdfridge_free(work);
      GSL_ERROR_VAL("failed to allocate space for weight vector",
                    GSL_ENOMEM, 0);
    }

  work->f = gsl_vector_alloc(n);
  if (work->f == NULL)
    {
      gsl_multifit_fdfridge_free(work);
      GSL_ERROR_VAL("failed to allocate space for f vector",
                    GSL_ENOMEM, 0);
    }

  work->n = n;
  work->p = p;
  work->lambda = 0.0;

  /* initialize weights to 1 (for augmented portion of vector) */
  gsl_vector_set_all(work->wts, 1.0);

  return work;
} /* gsl_multifit_fdfridge_alloc() */

void
gsl_multifit_fdfridge_free(gsl_multifit_fdfridge *work)
{
  if (work->s)
    gsl_multifit_fdfsolver_free(work->s);

  if (work->wts)
    gsl_vector_free(work->wts);

  if (work->f)
    gsl_vector_free(work->f);

  free(work);
}

const char *
gsl_multifit_fdfridge_name(const gsl_multifit_fdfridge * w)
{
  return gsl_multifit_fdfsolver_name(w->s);
}

gsl_vector *
gsl_multifit_fdfridge_position (const gsl_multifit_fdfridge * w)
{
  return gsl_multifit_fdfsolver_position(w->s);
}

gsl_vector *
gsl_multifit_fdfridge_residual (const gsl_multifit_fdfridge * w)
{
  return gsl_multifit_fdfsolver_residual(w->s);
}

size_t
gsl_multifit_fdfridge_niter (const gsl_multifit_fdfridge * w)
{
  return w->s->niter;
}

int
gsl_multifit_fdfridge_set (gsl_multifit_fdfridge * w,
                           gsl_multifit_function_fdf * f,
                           const gsl_vector * x,
                           const double lambda)
{
  return gsl_multifit_fdfridge_wset(w, f, x, lambda, NULL);
} /* gsl_multifit_fdfridge_set() */

int
gsl_multifit_fdfridge_wset (gsl_multifit_fdfridge * w,
                            gsl_multifit_function_fdf * f,
                            const gsl_vector * x,
                            const double lambda,
                            const gsl_vector * wts)
{
  const size_t n = w->n;
  const size_t p = w->p;

  if (n != f->n || p != f->p)
    {
      GSL_ERROR ("function size does not match solver", GSL_EBADLEN);
    }
  else if (p != x->size)
    {
      GSL_ERROR ("vector length does not match solver", GSL_EBADLEN);
    }
  else if (wts != NULL && n != wts->size)
    {
      GSL_ERROR ("weight vector length does not match solver", GSL_EBADLEN);
    }
  else
    {
      int status;
      gsl_vector_view wv = gsl_vector_subvector(w->wts, 0, n);

      /* save user defined fdf */
      w->fdf = f;

      /* build modified fdf for Tikhonov terms */
      w->fdftik.f = &fdfridge_f;
      w->fdftik.df = &fdfridge_df;
      w->fdftik.n = n + p; /* add p for Tikhonov terms */
      w->fdftik.p = p;
      w->fdftik.params = (void *) w;

      /* store damping parameter */
      w->lambda = lambda;
      w->L = NULL;

      if (wts)
        {
          /* copy weight vector into user portion of w->wts */
          gsl_vector_memcpy(&wv.vector, wts);
          status = gsl_multifit_fdfsolver_wset(w->s, &(w->fdftik), x, w->wts);
        }
      else
        {
          status = gsl_multifit_fdfsolver_wset(w->s, &(w->fdftik), x, NULL);
        }

      /* update function/Jacobian evaluations */
      f->nevalf = w->fdftik.nevalf;
      f->nevaldf = w->fdftik.nevaldf;

      return status;
    }
} /* gsl_multifit_fdfridge_wset() */

int
gsl_multifit_fdfridge_set2 (gsl_multifit_fdfridge * w,
                            gsl_multifit_function_fdf * f,
                            const gsl_vector * x,
                            const gsl_vector * lambda)
{
  return gsl_multifit_fdfridge_wset2(w, f, x, lambda, NULL);
} /* gsl_multifit_fdfridge_set2() */

int
gsl_multifit_fdfridge_wset2 (gsl_multifit_fdfridge * w,
                             gsl_multifit_function_fdf * f,
                             const gsl_vector * x,
                             const gsl_vector * lambda,
                             const gsl_vector * wts)
{
  const size_t n = w->n;
  const size_t p = w->p;

  if (n != f->n || p != f->p)
    {
      GSL_ERROR ("function size does not match solver", GSL_EBADLEN);
    }
  else if (p != x->size)
    {
      GSL_ERROR ("vector length does not match solver", GSL_EBADLEN);
    }
  else if (lambda->size != p)
    {
      GSL_ERROR ("lambda vector size does not match solver", GSL_EBADLEN);
    }
  else if (wts != NULL && n != wts->size)
    {
      GSL_ERROR ("weight vector length does not match solver", GSL_EBADLEN);
    }
  else
    {
      int status;
      gsl_vector_view wv = gsl_vector_subvector(w->wts, 0, n);

      /* save user defined fdf */
      w->fdf = f;
      w->fdf->nevalf = 0;
      w->fdf->nevaldf = 0;

      /* build modified fdf for Tikhonov terms */
      w->fdftik.f = &fdfridge_f;
      w->fdftik.df = &fdfridge_df;
      w->fdftik.n = n + p; /* add p for Tikhonov terms */
      w->fdftik.p = p;
      w->fdftik.params = (void *) w;

      /* store damping matrix */
      w->lambda = 0.0;
      w->L_diag = lambda;
      w->L = NULL;

      if (wts)
        {
          /* copy weight vector into user portion */
          gsl_vector_memcpy(&wv.vector, wts);
          status = gsl_multifit_fdfsolver_wset(w->s, &(w->fdftik), x, w->wts);
        }
      else
        {
          status = gsl_multifit_fdfsolver_wset(w->s, &(w->fdftik), x, NULL);
        }

      /* update function/Jacobian evaluations */
      f->nevalf = w->fdftik.nevalf;
      f->nevaldf = w->fdftik.nevaldf;

      return status;
    }
} /* gsl_multifit_fdfridge_wset2() */

int
gsl_multifit_fdfridge_set3 (gsl_multifit_fdfridge * w,
                            gsl_multifit_function_fdf * f,
                            const gsl_vector * x,
                            const gsl_matrix * L)
{
  return gsl_multifit_fdfridge_wset3(w, f, x, L, NULL);
} /* gsl_multifit_fdfridge_set3() */

int
gsl_multifit_fdfridge_wset3 (gsl_multifit_fdfridge * w,
                             gsl_multifit_function_fdf * f,
                             const gsl_vector * x,
                             const gsl_matrix * L,
                             const gsl_vector * wts)
{
  const size_t n = w->n;
  const size_t p = w->p;

  if (n != f->n || p != f->p)
    {
      GSL_ERROR ("function size does not match solver", GSL_EBADLEN);
    }
  else if (p != x->size)
    {
      GSL_ERROR ("vector length does not match solver", GSL_EBADLEN);
    }
  else if (L->size2 != p)
    {
      GSL_ERROR ("L matrix size2 does not match solver", GSL_EBADLEN);
    }
  else if (wts != NULL && n != wts->size)
    {
      GSL_ERROR ("weight vector length does not match solver", GSL_EBADLEN);
    }
  else
    {
      int status;
      gsl_vector_view wv = gsl_vector_subvector(w->wts, 0, n);

      /* save user defined fdf */
      w->fdf = f;
      w->fdf->nevalf = 0;
      w->fdf->nevaldf = 0;

      /* build modified fdf for Tikhonov terms */
      w->fdftik.f = &fdfridge_f;
      w->fdftik.df = &fdfridge_df;
      w->fdftik.n = n + p; /* add p for Tikhonov terms */
      w->fdftik.p = p;
      w->fdftik.params = (void *) w;

      /* store damping matrix */
      w->lambda = 0.0;
      w->L_diag = NULL;
      w->L = L;

      if (wts)
        {
          /* copy weight vector into user portion */
          gsl_vector_memcpy(&wv.vector, wts);
          status = gsl_multifit_fdfsolver_wset(w->s, &(w->fdftik), x, w->wts);
        }
      else
        {
          status = gsl_multifit_fdfsolver_wset(w->s, &(w->fdftik), x, NULL);
        }

      /* update function/Jacobian evaluations */
      f->nevalf = w->fdftik.nevalf;
      f->nevaldf = w->fdftik.nevaldf;

      return status;
    }
} /* gsl_multifit_fdfridge_wset3() */

int
gsl_multifit_fdfridge_iterate (gsl_multifit_fdfridge * w)
{
  int status = gsl_multifit_fdfsolver_iterate(w->s);

  /* update function/Jacobian evaluations */
  w->fdf->nevalf = w->fdftik.nevalf;
  w->fdf->nevaldf = w->fdftik.nevaldf;

  return status;
}

int
gsl_multifit_fdfridge_driver (gsl_multifit_fdfridge * w,
                              const size_t maxiter,
                              const double xtol,
                              const double gtol,
                              const double ftol,
                              int *info)
{
  int status = gsl_multifit_fdfsolver_driver(w->s, maxiter, xtol,
                                             gtol, ftol, info);
  return status;
} /* gsl_multifit_fdfridge_driver() */

/*
fdfridge_f()
  Callback function to provide residuals, including extra p
Tikhonov terms. The residual vector will have the form:

f~ = [     f     ]
     [ \lambda x ]

where f is the user supplied residuals, x are the model
parameters, and \lambda is the Tikhonov damping parameter

Inputs: x      - model parameters (size p)
        params - pointer to fdfridge workspace
        f      - (output) (n+p) vector to store f~
*/

static int
fdfridge_f(const gsl_vector * x, void * params, gsl_vector * f)
{
  int status;
  gsl_multifit_fdfridge *w = (gsl_multifit_fdfridge *) params;
  const size_t n = w->n;
  const size_t p = w->p;
  gsl_vector_view f_user = gsl_vector_subvector(f, 0, n);
  gsl_vector_view f_tik = gsl_vector_subvector(f, n, p);

  /* call user callback function to get residual vector f */
  status = gsl_multifit_eval_wf(w->fdf, x, NULL, &f_user.vector);
  if (status)
    return status;

  if (w->L_diag)
    {
      /* store diag(L_diag) x in Tikhonov portion of f~ */
      gsl_vector_memcpy(&f_tik.vector, x);
      gsl_vector_mul(&f_tik.vector, w->L_diag);
    }
  else if (w->L)
    {
      /* store Lx in Tikhonov portion of f~ */
      gsl_blas_dgemv(CblasNoTrans, 1.0, w->L, x, 0.0, &f_tik.vector);
    }
  else
    {
      /* store \lambda x in Tikhonov portion of f~ */
      gsl_vector_memcpy(&f_tik.vector, x);
      gsl_vector_scale(&f_tik.vector, w->lambda);
    }

  return GSL_SUCCESS;
} /* fdfridge_f() */

static int
fdfridge_df(const gsl_vector * x, void * params, gsl_matrix * J)
{
  int status;
  gsl_multifit_fdfridge *w = (gsl_multifit_fdfridge *) params;
  const size_t n = w->n;
  const size_t p = w->p;
  gsl_matrix_view J_user = gsl_matrix_submatrix(J, 0, 0, n, p);
  gsl_matrix_view J_tik = gsl_matrix_submatrix(J, n, 0, p, p);
  gsl_vector_view diag = gsl_matrix_diagonal(&J_tik.matrix);

  /* compute Jacobian */
  if (w->fdf->df)
    status = gsl_multifit_eval_wdf(w->fdf, x, NULL, &J_user.matrix);
  else
    {
      /* compute f(x) and then finite difference Jacobian */
      status = gsl_multifit_eval_wf(w->fdf, x, NULL, w->f);
      status += gsl_multifit_fdfsolver_dif_df(x, NULL, w->fdf, w->f,
                                              &J_user.matrix);
    }

  if (status)
    return status;

  if (w->L_diag)
    {
      /* store diag(L_diag) in Tikhonov portion of J */
      gsl_matrix_set_zero(&J_tik.matrix);
      gsl_vector_memcpy(&diag.vector, w->L_diag);
    }
  else if (w->L)
    {
      /* store L in Tikhonov portion of J */
      gsl_matrix_memcpy(&J_tik.matrix, w->L);
    }
  else
    {
      /* store \lambda I_p in Tikhonov portion of J */
      gsl_matrix_set_zero(&J_tik.matrix);
      gsl_vector_set_all(&diag.vector, w->lambda);
    }

  return GSL_SUCCESS;
} /* fdfridge_df() */


/*
** Copyright (c) 2008, 2009   Gerald I. Evenden
*/
static const char
RCS_ID[] = "$Id: proj_gdinverse.c,v 5.6 2009/05/17 19:41:47 gie Exp gie $";
/*
** Permission is hereby granted, free of charge, to any person obtaining
** a copy of this software and associated documentation files (the
** "Software"), to deal in the Software without restriction, including
** without limitation the rights to use, copy, modify, merge, publish,
** distribute, sublicense, and/or sell copies of the Software, and to
** permit persons to whom the Software is furnished to do so, subject to
** the following conditions:
**
** The above copyright notice and this permission notice shall be
** included in all copies or substantial portions of the Software.
**
** THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
** EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
** MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
** IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
** CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
** TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
** SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/
/** @file proj_gdinverse.c
 * @brief general inverse projection support
 * @author Gerald I. Evenden
 */
#include "config.h"
#include <stdlib.h>

#if HAVE_LIBGSL

#include <gsl/gsl_vector.h>
#include <gsl/gsl_multiroots.h>
#include <project.h>
#define LOOP_LIMIT 10

struct p_gsl { PROJ *P; double x, y; };
typedef struct p_gsl GP;
	static int
func_f(const gsl_vector *x, void *p, gsl_vector *f) {
//func_f(const gsl_vector *x, GP *p, gsl_vector *f) {
	PROJ_LP lp;

	lp.lam = gsl_vector_get(x,0);
	lp.phi = gsl_vector_get(x,1);
	PROJ_XY xy = (*((GP *)p)->P->fwd)(lp, ((GP *)p)->P);
	gsl_vector_set(f, 0, xy.x - ((GP *)p)->x);
	gsl_vector_set(f, 1, xy.y - ((GP *)p)->y);
	return GSL_SUCCESS;
}
	static int
func_df(const gsl_vector * x, void *p, gsl_matrix *J) {
	struct PROJ_DERIVS der;
	PROJ_LP lp;

	lp.lam = gsl_vector_get(x,0);
	lp.phi = gsl_vector_get(x,1);
	(*((GP *)p)->P->derivs)(((GP *)p)->P, lp, &der);
	gsl_matrix_set(J, 0, 0, der.x_l);
	gsl_matrix_set(J, 0, 1, der.x_p);
	gsl_matrix_set(J, 1, 0, der.y_l);
	gsl_matrix_set(J, 1, 1, der.y_p);
	return GSL_SUCCESS;
}
	static int
func_fdf(const gsl_vector * x, void *p, gsl_vector *f,  gsl_matrix *J) {
	func_f(x, p, f);
	func_df(x, p, J);
	return GSL_SUCCESS;
}
/* @brief determine projection inverse
 * @param[in] P projection control structure
 * @param est estimated geographic answer
 * @param[in] xy Cartesian location
 * @param[in] tol tolerance
 * @return ==0 if successful
 */
	int
proj_gdinverse(PROJ *P, PROJ_LP *est, PROJ_XY xy, double tol) {
	const gsl_multiroot_fdfsolver_type *T2;
	const gsl_multiroot_fsolver_type *T1;
	gsl_multiroot_fdfsolver *s2;
	gsl_multiroot_fsolver *s1;
	int status;
	size_t i, iter = 0;
	const size_t n = 2;
	GP p;
	int ders = P->derivs != NULL;
	gsl_multiroot_function_fdf f2 = {&func_f, &func_df, &func_fdf, n, &p};
	gsl_multiroot_function f1 = {&func_f, n, &p};

	p.P = P;
	p.x = xy.x;
	p.y = xy.y;
	gsl_vector *x = gsl_vector_alloc(n);
	gsl_vector_set(x, 0, est->lam);
	gsl_vector_set(x, 1, est->phi);
	if (ders) {
		T2 = gsl_multiroot_fdfsolver_gnewton;
		s2 = gsl_multiroot_fdfsolver_alloc(T2, n);
		gsl_multiroot_fdfsolver_set(s2, &f2, x);
	} else {
		T1 = gsl_multiroot_fsolver_hybrids;
		s1 = gsl_multiroot_fsolver_alloc(T1, n);
		gsl_multiroot_fsolver_set(s1, &f1, x);
	}
	do {
		++iter;
		status = ders ? gsl_multiroot_fdfsolver_iterate(s2):
						gsl_multiroot_fsolver_iterate(s1);
		if (status)
			break;
		status = gsl_multiroot_test_residual( ders?s2->f : s1->f, tol);
	} while (status == GSL_CONTINUE && iter < LOOP_LIMIT);
	if (ders) {
		est->lam = gsl_vector_get(s2->x,0);
		est->phi = gsl_vector_get(s2->x,1);
		gsl_multiroot_fdfsolver_free(s2);
	} else {
		est->lam = gsl_vector_get(s1->x,0);
		est->phi = gsl_vector_get(s1->x,1);
		gsl_multiroot_fsolver_free(s1);
	}
	gsl_vector_free(x);
	return 0;
}
#else // end LIBGSL true
/* the following is present to resolve any funky linking and to
 * ensure the system fails if gdinverse is called when the GSL flag
 * is not set.
*/
#include <stdio.h>
#include <project.h>
	int
proj_gdinverse(PROJ *P, PROJ_LP *est, PROJ_XY xy, double tol) {
	fprintf(stderr,"libproj4: entered gdinverse---no GSL avail. See admin\n");
	exit(-1);
	return -1;
}
#endif // end LIBGSL condition
/*
 * $Log:
 */


/* multilarge_nlinear/gsl_multilarge_nlinear.h
 *
 * Copyright (C) 2015, 2016 Patrick Alken
 *
 * 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 3 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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
 */

#ifndef __GSL_MULTILARGE_NLINEAR_H__
#define __GSL_MULTILARGE_NLINEAR_H__

#if !defined( GSL_FUN )
#  if !defined( GSL_DLL )
#    define GSL_FUN extern
#  elif defined( BUILD_GSL_DLL )
#    define GSL_FUN extern __declspec(dllexport)
#  else
#    define GSL_FUN extern __declspec(dllimport)
#  endif
#endif

#include <stdlib.h>
#include <gsl/gsl_types.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include <gsl/gsl_blas.h>

#undef __BEGIN_DECLS
#undef __END_DECLS
#ifdef __cplusplus
# define __BEGIN_DECLS extern "C" {
# define __END_DECLS }
#else
# define __BEGIN_DECLS /* empty */
# define __END_DECLS /* empty */
#endif

__BEGIN_DECLS

typedef enum
{
  GSL_MULTILARGE_NLINEAR_FWDIFF,
  GSL_MULTILARGE_NLINEAR_CTRDIFF
} gsl_multilarge_nlinear_fdtype;

/* Definition of vector-valued functions and gradient with parameters
   based on gsl_vector */

typedef struct
{
  int (* f) (const gsl_vector * x, void * params, gsl_vector * f);
  int (* df) (CBLAS_TRANSPOSE_t TransJ, const gsl_vector * x,
              const gsl_vector * u, void * params, gsl_vector * v,
              gsl_matrix * JTJ);
  int (* fvv) (const gsl_vector * x, const gsl_vector * v, void * params,
               gsl_vector * fvv);
  size_t n;        /* number of functions */
  size_t p;        /* number of independent variables */
  void * params;   /* user parameters */
  size_t nevalf;   /* number of function evaluations */
  size_t nevaldfu; /* number of Jacobian matrix-vector evaluations */
  size_t nevaldf2; /* number of Jacobian J^T J evaluations */
  size_t nevalfvv; /* number of fvv evaluations */
} gsl_multilarge_nlinear_fdf;

/* trust region subproblem method */
typedef struct
{
  const char *name;
  void * (*alloc) (const void * params, const size_t n, const size_t p);
  int (*init) (const void * vtrust_state, void * vstate);
  int (*preloop) (const void * vtrust_state, void * vstate);
  int (*step) (const void * vtrust_state, const double delta,
               gsl_vector * dx, void * vstate);
  int (*preduction) (const void * vtrust_state, const gsl_vector * dx,
                     double * pred, void * vstate);
  void (*free) (void * vstate);
} gsl_multilarge_nlinear_trs;

/* scaling matrix specification */
typedef struct
{
  const char *name;
  int (*init) (const gsl_matrix * JTJ, gsl_vector * diag);
  int (*update) (const gsl_matrix * JTJ, gsl_vector * diag);
} gsl_multilarge_nlinear_scale;

/*
 * linear least squares solvers - there are three steps to
 * solving a least squares problem using a direct method:
 *
 * 1. init: called once per iteration when a new Jacobian matrix
 *          is required; form normal equations matrix J^T J
 * 2. presolve: called each time a new LM parameter value mu is available;
 *              used for cholesky method in order to factor
 *              the (J^T J + mu D^T D) matrix
 * 3. solve: solve the least square system for a given rhs
 */
typedef struct
{
  const char *name;
  void * (*alloc) (const size_t n, const size_t p);
  int (*init) (const void * vtrust_state, void * vstate);
  int (*presolve) (const double mu, const void * vtrust_state, void * vstate);
  int (*solve) (const gsl_vector * g, gsl_vector * x,
                const void * vtrust_state, void * vstate);
  int (*rcond) (double * rcond, const gsl_matrix * JTJ, void * vstate);
  int (*covar) (const gsl_matrix * JTJ, gsl_matrix * covar, void * vstate);
  void (*free) (void * vstate);
} gsl_multilarge_nlinear_solver;

/* tunable parameters */
typedef struct
{
  const gsl_multilarge_nlinear_trs *trs;       /* trust region subproblem method */
  const gsl_multilarge_nlinear_scale *scale;   /* scaling method */
  const gsl_multilarge_nlinear_solver *solver; /* solver method */
  gsl_multilarge_nlinear_fdtype fdtype;        /* finite difference method */
  double factor_up;                            /* factor for increasing trust radius */
  double factor_down;                          /* factor for decreasing trust radius */
  double avmax;                                /* max allowed |a|/|v| */
  double h_df;                                 /* step size for finite difference Jacobian */
  double h_fvv;                                /* step size for finite difference fvv */
  size_t max_iter;                             /* maximum iterations for trs method */
  double tol;                                  /* tolerance for solving trs */
} gsl_multilarge_nlinear_parameters;

typedef struct
{
  const char *name;
  void * (*alloc) (const gsl_multilarge_nlinear_parameters * params,
                   const size_t n, const size_t p);
  int (*init) (void * state, const gsl_vector * wts,
               gsl_multilarge_nlinear_fdf * fdf, const gsl_vector * x,
               gsl_vector * f, gsl_vector * g, gsl_matrix * JTJ);
  int (*iterate) (void * state, const gsl_vector * wts,
                  gsl_multilarge_nlinear_fdf * fdf, gsl_vector * x,
                  gsl_vector * f, gsl_vector * g, gsl_matrix * JTJ,
                  gsl_vector * dx);
  int (*rcond) (double * rcond, const gsl_matrix * JTJ, void * state);
  int (*covar) (const gsl_matrix * JTJ, gsl_matrix * covar, void * state);
  double (*avratio) (void * state);
  void (*free) (void * state);
} gsl_multilarge_nlinear_type;

/* current state passed to low-level trust region algorithms */
typedef struct
{
  const gsl_vector * x;             /* parameter values x */
  const gsl_vector * f;             /* residual vector f(x) */
  const gsl_vector * g;             /* gradient J^T f */
  const gsl_matrix * JTJ;           /* matrix J^T J */
  const gsl_vector * diag;          /* scaling matrix D */
  const gsl_vector * sqrt_wts;      /* sqrt(diag(W)) or NULL for unweighted */
  const double *mu;                 /* LM parameter */
  const gsl_multilarge_nlinear_parameters * params;
  void *solver_state;               /* workspace for direct least squares solver */
  gsl_multilarge_nlinear_fdf * fdf;
  double *avratio;                  /* |a| / |v| */
} gsl_multilarge_nlinear_trust_state;

typedef struct
{
  const gsl_multilarge_nlinear_type * type;
  gsl_multilarge_nlinear_fdf * fdf ;
  gsl_vector * x;             /* parameter values x */
  gsl_vector * f;             /* residual vector f(x) */
  gsl_vector * dx;            /* step dx */
  gsl_vector * g;             /* gradient J^T f */
  gsl_matrix * JTJ;           /* matrix J^T J */
  gsl_vector * sqrt_wts_work; /* sqrt(W) */
  gsl_vector * sqrt_wts;      /* ptr to sqrt_wts_work, or NULL if not using weights */
  size_t n;                   /* number of residuals */
  size_t p;                   /* number of parameters */
  size_t niter;               /* number of iterations performed */
  gsl_multilarge_nlinear_parameters params;
  void *state;
} gsl_multilarge_nlinear_workspace;

GSL_FUN gsl_multilarge_nlinear_workspace *
gsl_multilarge_nlinear_alloc (const gsl_multilarge_nlinear_type * T,
                              const gsl_multilarge_nlinear_parameters * params,
                              size_t n, size_t p);

GSL_FUN void gsl_multilarge_nlinear_free (gsl_multilarge_nlinear_workspace * w);

GSL_FUN gsl_multilarge_nlinear_parameters gsl_multilarge_nlinear_default_parameters(void);

GSL_FUN int
gsl_multilarge_nlinear_init (const gsl_vector * x,
                             gsl_multilarge_nlinear_fdf * fdf,
                             gsl_multilarge_nlinear_workspace * w);

GSL_FUN int gsl_multilarge_nlinear_winit (const gsl_vector * x,
                                  const gsl_vector * wts,
                                  gsl_multilarge_nlinear_fdf * fdf,
                                  gsl_multilarge_nlinear_workspace * w);

GSL_FUN int
gsl_multilarge_nlinear_iterate (gsl_multilarge_nlinear_workspace * w);

GSL_FUN double
gsl_multilarge_nlinear_avratio (const gsl_multilarge_nlinear_workspace * w);

GSL_FUN int
gsl_multilarge_nlinear_rcond (double * rcond, const gsl_multilarge_nlinear_workspace * w);

GSL_FUN int
gsl_multilarge_nlinear_covar (gsl_matrix * covar, gsl_multilarge_nlinear_workspace * w);

GSL_FUN int
gsl_multilarge_nlinear_driver (const size_t maxiter,
                               const double xtol,
                               const double gtol,
                               const double ftol,
                               void (*callback)(const size_t iter, void *params,
                                                const gsl_multilarge_nlinear_workspace *w),
                               void *callback_params,
                               int *info,
                               gsl_multilarge_nlinear_workspace * w);

GSL_FUN const char *
gsl_multilarge_nlinear_name (const gsl_multilarge_nlinear_workspace * w);

GSL_FUN gsl_vector *
gsl_multilarge_nlinear_position (const gsl_multilarge_nlinear_workspace * w);

GSL_FUN gsl_vector *
gsl_multilarge_nlinear_residual (const gsl_multilarge_nlinear_workspace * w);

GSL_FUN gsl_vector *
gsl_multilarge_nlinear_step (const gsl_multilarge_nlinear_workspace * w);

GSL_FUN size_t
gsl_multilarge_nlinear_niter (const gsl_multilarge_nlinear_workspace * w);

GSL_FUN const char *
gsl_multilarge_nlinear_trs_name (const gsl_multilarge_nlinear_workspace * w);

GSL_FUN int gsl_multilarge_nlinear_eval_f(gsl_multilarge_nlinear_fdf *fdf,
                                  const gsl_vector *x,
                                  const gsl_vector *swts,
                                  gsl_vector *y);

GSL_FUN int
gsl_multilarge_nlinear_eval_df(const CBLAS_TRANSPOSE_t TransJ,
                               const gsl_vector *x,
                               const gsl_vector *f,
                               const gsl_vector *u,
                               const gsl_vector *swts,
                               const double h,
                               const gsl_multilarge_nlinear_fdtype fdtype,
                               gsl_multilarge_nlinear_fdf *fdf,
                               gsl_vector *v,
                               gsl_matrix *JTJ,
                               gsl_vector *work);

GSL_FUN int
gsl_multilarge_nlinear_eval_fvv(const double h,
                                const gsl_vector *x,
                                const gsl_vector *v,
                                const gsl_vector *f,
                                const gsl_vector *swts,
                                gsl_multilarge_nlinear_fdf *fdf,
                                gsl_vector *yvv,
                                gsl_vector *work);

/* convergence.c */
GSL_FUN int
gsl_multilarge_nlinear_test (const double xtol, const double gtol,
                             const double ftol, int *info,
                             const gsl_multilarge_nlinear_workspace * w);

/* fdjac.c */
GSL_FUN int
gsl_multilarge_nlinear_df(const double h, const gsl_multilarge_nlinear_fdtype fdtype,
                          const gsl_vector *x, const gsl_vector *wts,
                          gsl_multilarge_nlinear_fdf *fdf,
                          const gsl_vector *f, gsl_matrix *J, gsl_vector *work);

/* fdfvv.c */
GSL_FUN int
gsl_multilarge_nlinear_fdfvv(const double h, const gsl_vector *x, const gsl_vector *v,
                             const gsl_vector *f, const gsl_matrix *J,
                             const gsl_vector *swts, gsl_multilarge_nlinear_fdf *fdf,
                             gsl_vector *fvv, gsl_vector *work);

/* top-level algorithms */
GSL_VAR const gsl_multilarge_nlinear_type * gsl_multilarge_nlinear_trust;

/* trust region subproblem methods */
GSL_VAR const gsl_multilarge_nlinear_trs * gsl_multilarge_nlinear_trs_lm;
GSL_VAR const gsl_multilarge_nlinear_trs * gsl_multilarge_nlinear_trs_lmaccel;
GSL_VAR const gsl_multilarge_nlinear_trs * gsl_multilarge_nlinear_trs_dogleg;
GSL_VAR const gsl_multilarge_nlinear_trs * gsl_multilarge_nlinear_trs_ddogleg;
GSL_VAR const gsl_multilarge_nlinear_trs * gsl_multilarge_nlinear_trs_subspace2D;
GSL_VAR const gsl_multilarge_nlinear_trs * gsl_multilarge_nlinear_trs_cgst;

/* scaling matrix strategies */
GSL_VAR const gsl_multilarge_nlinear_scale * gsl_multilarge_nlinear_scale_levenberg;
GSL_VAR const gsl_multilarge_nlinear_scale * gsl_multilarge_nlinear_scale_marquardt;
GSL_VAR const gsl_multilarge_nlinear_scale * gsl_multilarge_nlinear_scale_more;

/* linear solvers */
GSL_VAR const gsl_multilarge_nlinear_solver * gsl_multilarge_nlinear_solver_cholesky;
GSL_VAR const gsl_multilarge_nlinear_solver * gsl_multilarge_nlinear_solver_mcholesky;
GSL_VAR const gsl_multilarge_nlinear_solver * gsl_multilarge_nlinear_solver_none;

__END_DECLS

#endif /* __GSL_MULTILARGE_NLINEAR_H__ */


#include <blas.h>
#include <inttypes.h>
#include "mex.h"
#include "mx_blas.h"

/* Level 1 BLAS */
void
mx_swap(mxArray *mxx, mxArray *mxy)
{
    const ptrdiff_t incx = 1;
    const ptrdiff_t incy = 1;
    const ptrdiff_t n = (const ptrdiff_t)mxGetNumberOfElements(mxx);

    if (mxIsSingle(mxx)) {
        float *x = (float *)mxGetData(mxx);
        float *y = (float *)mxGetData(mxy);
        sswap(&n, x, &incx, y, &incy);

    } else if (mxIsDouble(mxx)) {
        double *x = (double *)mxGetData(mxx);
        double *y = (double *)mxGetData(mxy);
        dswap(&n, x, &incx, y, &incy);

    } else {
        mexErrMsgTxt("not implemented");
    }

    return;
} /* mx_swap() */

void
mx_scal(const double a, mxArray *mxx)
{
    const ptrdiff_t incx = 1;
    const ptrdiff_t n = (const ptrdiff_t)mxGetNumberOfElements(mxx);

    if (mxIsSingle(mxx)) {
        const float af = (const float)a;
        float *x = (float *)mxGetData(mxx);
        sscal(&n, &af, x, &incx);

    } else if (mxIsDouble(mxx)) {
        double *x = (double *)mxGetData(mxx);
        dscal(&n, &a, x, &incx);

    } else {
        mexErrMsgTxt("not implemented");
    }

    return;
} /* mx_scal() */

void
mx_copy(const mxArray *mxx, mxArray *mxy)
{
    const ptrdiff_t incx = 1;
    const ptrdiff_t incy = 1;
    const ptrdiff_t n = (const ptrdiff_t)mxGetNumberOfElements(mxx);

    if (mxIsSingle(mxx)) {
        float *y = (float *)mxGetData(mxy);
        const float *x = (const float *)mxGetData(mxx);
        scopy(&n, x, &incx, y, &incy);

    } else if (mxIsDouble(mxx)) {
        double *y = (double *)mxGetData(mxy);
        const double *x = (const double *)mxGetData(mxx);
        dcopy(&n, x, &incx, y, &incy);

    } else {
        mexErrMsgTxt("not implemented");
    }

    return;
} /* mx_copy() */

void
mx_axpy(const double a, const mxArray *mxx, mxArray *mxy)
{
    const ptrdiff_t incx = 1;
    const ptrdiff_t incy = 1;
    const ptrdiff_t n = (const ptrdiff_t)mxGetNumberOfElements(mxx);

    if (mxIsSingle(mxx)) {
        float *y = (float *)mxGetData(mxy);
        const float *x = (const float *)mxGetData(mxx);
        const float af = (const float)a;
        saxpy(&n, &af, x, &incx, y, &incy);

    } else if (mxIsDouble(mxx)) {
        double *y = (double *)mxGetData(mxy);
        const double *x = (const double *)mxGetData(mxx);
        daxpy(&n, &a, x, &incx, y, &incy);

    } else {
        mexErrMsgTxt("not implemented");
    }

    return;
} /* mx_axpy() */

double
mx_dot(const mxArray *mxx, const mxArray *mxy)
{
    double p = -1.0;

    const ptrdiff_t incx = 1;
    const ptrdiff_t incy = 1;
    const ptrdiff_t n = (const ptrdiff_t)mxGetNumberOfElements(mxx);

    if (mxIsSingle(mxx)) {
        const float *x = (const float *)mxGetData(mxx);
        const float *y = (const float *)mxGetData(mxy);
        p = (double)sdot(&n, x, &incx, y, &incy);

    } else if (mxIsDouble(mxx)) {
        const double *x = (const double *)mxGetData(mxx);
        const double *y = (const double *)mxGetData(mxy);
        p = ddot(&n, x, &incx, y, &incy);

    } else {
        mexErrMsgTxt("not implemented");
    }

    return p;
} /* mx_dot() */

double
mx_nrm2(const mxArray *mxx)
{
    double p = -1.0;

    const ptrdiff_t incx = 1;
    const ptrdiff_t n = (const ptrdiff_t)mxGetNumberOfElements(mxx);

    if (mxIsSingle(mxx)) {
        const float *x = (const float *)mxGetData(mxx);
        p = (double)snrm2(&n, x, &incx);

    } else if (mxIsDouble(mxx)) {
        const double *x = (const double *)mxGetData(mxx);
        p = dnrm2(&n, x, &incx);

    } else {
        mexErrMsgTxt("not implemented");
    }

    return p;
} /* mx_nrm2() */

double
mx_asum(const mxArray *mxx)
{
    double p = -1.0;

    const ptrdiff_t incx = 1;
    const ptrdiff_t n = (const ptrdiff_t)mxGetNumberOfElements(mxx);

    if (mxIsSingle(mxx)) {
        const float *x = (const float *)mxGetData(mxx);
        p = (double)sasum(&n, x, &incx);

    } else if (mxIsDouble(mxx)) {
        const double *x = (const double *)mxGetData(mxx);
        p = dasum(&n, x, &incx);

    } else {
        mexErrMsgTxt("not implemented");
    }

    return p;
} /* mx_asum() */