/* interpolation/test.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000 Gerard Jungman
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or (at
 * your option) any later version.
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
 */

/* Author:  G. Jungman
 */
#include <config.h>
#include <stddef.h>
#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_test.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_interp.h>

int
test_bsearch(void)
{
  double x_array[5] = { 0.0, 1.0, 2.0, 3.0, 4.0 };
  size_t index_result;
  int status = 0;
  int s;

  /* check an interior point */
  index_result = gsl_interp_bsearch(x_array, 1.5, 0, 4);
  s = (index_result != 1);
  status += s;
  gsl_test (s, "simple bsearch");

  /* check that we get the last interval if x == last value */
  index_result = gsl_interp_bsearch(x_array, 4.0, 0, 4);
  s = (index_result != 3);
  status += s;
  gsl_test (s, "upper endpoint bsearch");

  /* check that we get the first interval if x == first value */
  index_result = gsl_interp_bsearch(x_array, 0.0, 0, 4);
  s = (index_result != 0);
  status += s;
  gsl_test (s, "lower endpoint bsearch");

  /* check that we get correct interior boundary behaviour */
  index_result = gsl_interp_bsearch(x_array, 2.0, 0, 4);
  s = (index_result != 2);
  status += s;
  gsl_test (s, "degenerate bsearch");

  /* check out of bounds above */
  index_result = gsl_interp_bsearch(x_array, 10.0, 0, 4);
  s = (index_result != 3);
  status += s;
  gsl_test (s, "out of bounds bsearch +");

  /* check out of bounds below */
  index_result = gsl_interp_bsearch(x_array, -10.0, 0, 4);
  s = (index_result != 0);
  status += s;
  gsl_test (s, "out of bounds bsearch -");

  return status;
}

typedef double TEST_FUNC (double);
typedef struct _xy_table xy_table;

struct _xy_table
  {
    double * x;
    double * y;
    size_t n;
  };

xy_table make_xy_table (double x[], double y[], size_t n);

xy_table
make_xy_table (double x[], double y[], size_t n)
{
  xy_table t;
  t.x = x;
  t.y = y;
  t.n = n;
  return t;
}

static int
test_interp (
  const xy_table * data_table,
  const gsl_interp_type * T,
  xy_table * test_table,
  xy_table * test_d_table,
  xy_table * test_i_table
  )
{
  int status = 0;
  size_t i;

  gsl_interp_accel *a = gsl_interp_accel_alloc ();
  gsl_interp *interp = gsl_interp_alloc (T, data_table->n);

  gsl_interp_init (interp, data_table->x, data_table->y, data_table->n);

  for (i = 0; i < test_table->n; i++)
    {
      double x = test_table->x[i];
      double y;
      double deriv;
      double integ;
      double diff_y, diff_deriv, diff_integ;
      gsl_interp_eval_e (interp, data_table->x, data_table->y, x, a, &y);
      gsl_interp_eval_deriv_e (interp, data_table->x, data_table->y, x, a, &deriv);
      gsl_interp_eval_integ_e (interp, data_table->x, data_table->y, 0.0, x, a, &integ);
      diff_y = y - test_table->y[i];
      diff_deriv = deriv - test_d_table->y[i];
      diff_integ = integ - test_i_table->y[i];
      if (fabs (diff_y) > 1.e-10 || fabs(diff_deriv) > 1.0e-10 || fabs(diff_integ) > 1.0e-10) {
	status++;
      }
    }

  gsl_interp_accel_free (a);
  gsl_interp_free (interp);

  return status;
}

static int
test_linear (void)
{
  int s;

  double data_x[4] = { 0.0, 1.0, 2.0, 3.0 };
  double data_y[4] = { 0.0, 1.0, 2.0, 3.0 };
  double test_x[6] = { 0.0, 0.5, 1.0, 1.5, 2.5, 3.0 };
  double test_y[6] = { 0.0, 0.5, 1.0, 1.5, 2.5, 3.0 };
  double test_dy[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
  double test_iy[6] = { 0.0, 0.125, 0.5, 9.0/8.0, 25.0/8.0, 9.0/2.0 };

  xy_table data_table = make_xy_table(data_x, data_y, 4);
  xy_table test_table = make_xy_table(test_x, test_y, 6);
  xy_table test_d_table = make_xy_table(test_x, test_dy, 6);
  xy_table test_i_table = make_xy_table(test_x, test_iy, 6);

  s = test_interp (&data_table, gsl_interp_linear, &test_table, &test_d_table, &test_i_table);
  gsl_test (s, "linear interpolation");
  return s;
}

static int
test_cspline (void)
{
  int s;

  double data_x[3] = { 0.0, 1.0, 2.0 };
  double data_y[3] = { 0.0, 1.0, 2.0 };
  double test_x[4] = { 0.0, 0.5, 1.0, 2.0 };
  double test_y[4] = { 0.0, 0.5, 1.0, 2.0 };
  double test_dy[4] = { 1.0, 1.0, 1.0, 1.0 };
  double test_iy[4] = { 0.0, 0.125, 0.5, 2.0 };

  xy_table data_table = make_xy_table(data_x, data_y, 3);
  xy_table test_table = make_xy_table(test_x, test_y, 4);
  xy_table test_d_table = make_xy_table(test_x, test_dy, 4);
  xy_table test_i_table = make_xy_table(test_x, test_iy, 4);

  s = test_interp (&data_table, gsl_interp_cspline, &test_table, &test_d_table, &test_i_table);
  gsl_test (s, "cspline interpolation");
  return s;
}

static int
test_akima (void)
{
  int s;

  double data_x[5] = { 0.0, 1.0, 2.0, 3.0, 4.0 };
  double data_y[5] = { 0.0, 1.0, 2.0, 3.0, 4.0 };
  double test_x[4] = { 0.0, 0.5, 1.0, 2.0 };
  double test_y[4] = { 0.0, 0.5, 1.0, 2.0 };
  double test_dy[4] = { 1.0, 1.0, 1.0, 1.0 };
  double test_iy[4] = { 0.0, 0.125, 0.5, 2.0 };

  xy_table data_table = make_xy_table(data_x, data_y, 5);
  xy_table test_table = make_xy_table(test_x, test_y, 4);
  xy_table test_d_table = make_xy_table(test_x, test_dy, 4);
  xy_table test_i_table = make_xy_table(test_x, test_iy, 4);

  s = test_interp (&data_table, gsl_interp_akima, &test_table, &test_d_table, &test_i_table);
  gsl_test (s, "akima interpolation");
  return s;
}

int
main (int argc, char **argv)
{
  int status = 0;

  argc = 0;    /* prevent warnings about unused parameters */
  argv = 0;

  status += test_bsearch();
  status += test_linear();
  status += test_cspline();
  status += test_akima();

  exit (gsl_test_summary());
}


/// FicTrac http://rjdmoore.net/fictrac/
/// \file       NLoptFunc.h
/// \brief      Wrapper class for NLopt.
/// \author     Saul Thurrowgood, Richard Moore
/// \copyright  CC BY-NC-SA 3.0

#pragma once

#include <nlopt.h>

#include <vector>

///
/// C++ wrapper of the NLopt library for nonlinear optimisation, v2.2+.
/// See:  http://ab-initio.mit.edu/nlopt
///
class NLoptFunc
{
public:
	NLoptFunc();
	virtual ~NLoptFunc();

	///
	/// Run the optimiser.  Default behaviour is to start the search at the
	/// position of the previous result.
	///
	virtual nlopt_result optimize(const double *xInit=0);

	///
	/// Sets the initial value used for the next call to optimize(),
	/// which assumes that call passes NULL as the xInit argument.
	///
	void setXInit(const double *xInit);

	///
	/// Get the optimised value.
	///
	void getOptX(float *x);
	void getOptX(double *x);
	double getOptF();

	///
	/// Returns the number of objective() evaluations by the last optimisation.
	///
	unsigned getNumEval();

	///
	/// Initialise the optimiser.  If 'minimise' is false then the objective
	/// function will be maximised.
	///
	void init(nlopt_algorithm algo, unsigned n, bool minimise=true);

    void setPopulation(unsigned int pop);

	///
	/// Set initial step size.
	///
	void setInitialStep(const double *dx);

	///
	/// Set stopping criteria.
	///
	void setLowerBounds(const double *lb);
	void setUpperBounds(const double *ub);
	void setLowerBounds(double lb); /// convenience: sets all to same value
	void setUpperBounds(double ub);
	void setFtol(double tol);   // relative
	void setXtol(double tol);   // absolute
    void setXtolRel(double tol);
	void setMaxEval(unsigned n);

	void getLowerBounds(double *lb);
	void getUpperBounds(double *ub);

	unsigned getDimension();

protected:
	unsigned _nEval;

	///
	/// Objective function executed during optimisation.
	///
	virtual double objective(unsigned n, const double *x, double *grad) = 0;

private:
	static double _cb(unsigned n, const double *x, double *grad, void *data);

	nlopt_opt _opt;
	std::vector<double> _x;
	double _optF;
};

Unused Code/FicTrac v2.0 (Not Working Well !!)/fictrac2_source/include/NLoptFunc.h


/* multifit/lmpar.c
 *
 * Copyright (C) 1996, 1997, 1998, 1999, 2000, 2007 Brian Gough
 *
 * 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 <gsl/gsl_permute_vector_double.h>

#include "qrsolv.c"

static size_t
count_nsing (const gsl_matrix * r)
{
  /* Count the number of nonsingular entries. Returns the index of the
     first entry which is singular. */

  size_t n = r->size2;
  size_t i;

  for (i = 0; i < n; i++)
    {
      double rii = gsl_matrix_get (r, i, i);

      if (rii == 0)
        {
          break;
        }
    }

  return i;
}

static void
compute_newton_direction (const gsl_matrix * r, const gsl_permutation * perm,
                          const gsl_vector * qtf, gsl_vector * x)
{

  /* Compute and store in x the Gauss-Newton direction. If the
     Jacobian is rank-deficient then obtain a least squares
     solution. */

  const size_t n = r->size2;
  size_t i, j, nsing;

  for (i = 0 ; i < n ; i++)
    {
      double qtfi = gsl_vector_get (qtf, i);
      gsl_vector_set (x, i,  qtfi);
    }

  nsing = count_nsing (r);

#ifdef DEBUG
  printf("nsing = %d\n", nsing);
  printf("r = "); gsl_matrix_fprintf(stdout, r, "%g"); printf("\n");
  printf("qtf = "); gsl_vector_fprintf(stdout, x, "%g"); printf("\n");
#endif

  for (i = nsing; i < n; i++)
    {
      gsl_vector_set (x, i, 0.0);
    }

  if (nsing > 0)
    {
      for (j = nsing; j > 0 && j--;)
        {
          double rjj = gsl_matrix_get (r, j, j);
          double temp = gsl_vector_get (x, j) / rjj;

          gsl_vector_set (x, j, temp);

          for (i = 0; i < j; i++)
            {
              double rij = gsl_matrix_get (r, i, j);
              double xi = gsl_vector_get (x, i);
              gsl_vector_set (x, i, xi - rij * temp);
            }
        }
    }

  gsl_permute_vector_inverse (perm, x);
}

static void
compute_newton_correction (const gsl_matrix * r, const gsl_vector * sdiag,
                           const gsl_permutation * p, gsl_vector * x,
                           double dxnorm,
                           const gsl_vector * diag, gsl_vector * w)
{
  size_t n = r->size2;
  size_t i, j;

  for (i = 0; i < n; i++)
    {
      size_t pi = gsl_permutation_get (p, i);

      double dpi = gsl_vector_get (diag, pi);
      double xpi = gsl_vector_get (x, pi);

      gsl_vector_set (w, i, dpi * (dpi * xpi) / dxnorm);
    }

  for (j = 0; j < n; j++)
    {
      double sj = gsl_vector_get (sdiag, j);
      double wj = gsl_vector_get (w, j);

      double tj = wj / sj;

      gsl_vector_set (w, j, tj);

      for (i = j + 1; i < n; i++)
        {
          double rij = gsl_matrix_get (r, i, j);
          double wi = gsl_vector_get (w, i);

          gsl_vector_set (w, i, wi - rij * tj);
        }
    }
}

static void
compute_newton_bound (const gsl_matrix * r, const gsl_vector * x,
                      double dxnorm,  const gsl_permutation * perm,
                      const gsl_vector * diag, gsl_vector * w)
{
  /* If the jacobian is not rank-deficient then the Newton step
     provides a lower bound for the zero of the function. Otherwise
     set this bound to zero. */

  size_t n = r->size2;

  size_t i, j;

  size_t nsing = count_nsing (r);

  if (nsing < n)
    {
      gsl_vector_set_zero (w);
      return;
    }

  for (i = 0; i < n; i++)
    {
      size_t pi = gsl_permutation_get (perm, i);

      double dpi = gsl_vector_get (diag, pi);
      double xpi = gsl_vector_get (x, pi);

      gsl_vector_set (w, i, dpi * (dpi * xpi / dxnorm));
    }

  for (j = 0; j < n; j++)
    {
      double sum = 0;

      for (i = 0; i < j; i++)
        {
          sum += gsl_matrix_get (r, i, j) * gsl_vector_get (w, i);
        }

      {
        double rjj = gsl_matrix_get (r, j, j);
        double wj = gsl_vector_get (w, j);

        gsl_vector_set (w, j, (wj - sum) / rjj);
      }
    }
}

/* compute scaled gradient g = D^{-1} J^T f (see More' eq 7.2) */
static void
compute_gradient_direction (const gsl_matrix * r, const gsl_permutation * p,
                            const gsl_vector * qtf, const gsl_vector * diag,
                            gsl_vector * g)
{
  const size_t n = r->size2;

  size_t i, j;

  for (j = 0; j < n; j++)
    {
      double sum = 0;

      for (i = 0; i <= j; i++)
        {
          sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i);
        }

      {
        size_t pj = gsl_permutation_get (p, j);
        double dpj = gsl_vector_get (diag, pj);

        gsl_vector_set (g, j, sum / dpj);
      }
    }
}

/* compute gradient g = J^T f */
static void
compute_gradient (const gsl_matrix * r, const gsl_vector * qtf,
                  gsl_vector * g)
{
  const size_t n = r->size2;

  size_t i, j;

  for (j = 0; j < n; j++)
    {
      double sum = 0;

      for (i = 0; i <= j; i++)
        {
          sum += gsl_matrix_get (r, i, j) * gsl_vector_get (qtf, i);
        }

      gsl_vector_set (g, j, sum);
    }
}

static int
lmpar (gsl_matrix * r, const gsl_permutation * perm, const gsl_vector * qtf,
       const gsl_vector * diag, double delta, double * par_inout,
       gsl_vector * newton, gsl_vector * gradient, gsl_vector * sdiag,
       gsl_vector * x, gsl_vector * w)
{
  double dxnorm, gnorm, fp, fp_old, par_lower, par_upper, par_c;

  double par = *par_inout;

  size_t iter = 0;

#ifdef DEBUG
  printf("ENTERING lmpar\n");
#endif

  compute_newton_direction (r, perm, qtf, newton);

#ifdef DEBUG
  printf ("newton = ");
  gsl_vector_fprintf (stdout, newton, "%g");
  printf ("\n");

  printf ("diag = ");
  gsl_vector_fprintf (stdout, diag, "%g");
  printf ("\n");
#endif

  /* Evaluate the function at the origin and test for acceptance of
     the Gauss-Newton direction. */

  dxnorm = scaled_enorm (diag, newton);

  fp = dxnorm - delta;

#ifdef DEBUG
  printf ("dxnorm = %g, delta = %g, fp = %g\n", dxnorm, delta, fp);
#endif

  if (fp <= 0.1 * delta)
    {
      gsl_vector_memcpy (x, newton);
#ifdef DEBUG
      printf ("took newton (fp = %g, delta = %g)\n", fp, delta);
#endif

      *par_inout = 0;

      return GSL_SUCCESS;
    }

#ifdef DEBUG
  printf ("r = ");
  gsl_matrix_fprintf (stdout, r, "%g");
  printf ("\n");

  printf ("newton = ");
  gsl_vector_fprintf (stdout, newton, "%g");
  printf ("\n");

  printf ("dxnorm = %g\n", dxnorm);
#endif

  compute_newton_bound (r, newton, dxnorm, perm, diag, w);

#ifdef DEBUG
  printf("perm = "); gsl_permutation_fprintf(stdout, perm, "%d");

  printf ("diag = ");
  gsl_vector_fprintf (stdout, diag, "%g");
  printf ("\n");

  printf ("w = ");
  gsl_vector_fprintf (stdout, w, "%g");
  printf ("\n");
#endif

  {
    double wnorm = enorm (w);
    double phider = wnorm * wnorm;

    /* w == zero if r rank-deficient,
       then set lower bound to zero form MINPACK, lmder.f
       Hans E. Plesser 2002-02-25 (hans.plesser@itf.nlh.no) */
    if ( wnorm > 0 )
      par_lower = fp / (delta * phider);
    else
      par_lower = 0.0;
  }

#ifdef DEBUG
  printf("par       = %g\n", par      );
  printf("par_lower = %g\n", par_lower);
#endif

  compute_gradient_direction (r, perm, qtf, diag, gradient);

  gnorm = enorm (gradient);

#ifdef DEBUG
  printf("gradient = "); gsl_vector_fprintf(stdout, gradient, "%g"); printf("\n");
  printf("gnorm = %g\n", gnorm);
#endif

  par_upper =  gnorm / delta;

  if (par_upper == 0)
    {
      par_upper = GSL_DBL_MIN / GSL_MIN_DBL(delta, 0.1);
    }

#ifdef DEBUG
  printf("par_upper = %g\n", par_upper);
#endif

  if (par > par_upper)
    {
#ifdef DEBUG
  printf("set par to par_upper\n");
#endif

      par = par_upper;
    }
  else if (par < par_lower)
    {
#ifdef DEBUG
  printf("set par to par_lower\n");
#endif

      par = par_lower;
    }

  if (par == 0)
    {
      par = gnorm / dxnorm;
#ifdef DEBUG
      printf("set par to gnorm/dxnorm = %g\n", par);
#endif

    }

  /* Beginning of iteration */

iteration:

  iter++;

#ifdef DEBUG
  printf("lmpar iteration = %d\n", iter);
#endif

#ifdef BRIANSFIX
  /* Seems like this is described in the paper but not in the MINPACK code */

  if (par < par_lower || par > par_upper)
    {
      par = GSL_MAX_DBL (0.001 * par_upper, sqrt(par_lower * par_upper));
    }
#endif

  /* Evaluate the function at the current value of par */

  if (par == 0)
    {
      par = GSL_MAX_DBL (0.001 * par_upper, GSL_DBL_MIN);
#ifdef DEBUG
      printf("par = 0, set par to  = %g\n", par);
#endif

    }

  /* Compute the least squares solution of [ R P x - Q^T f, sqrt(par) D x]
     for A = Q R P^T */

#ifdef DEBUG
  printf ("calling qrsolv with par = %g\n", par);
#endif

  {
    double sqrt_par = sqrt(par);

    qrsolv (r, perm, sqrt_par, diag, qtf, x, sdiag, w);
  }

  dxnorm = scaled_enorm (diag, x);

  fp_old = fp;

  fp = dxnorm - delta;

#ifdef DEBUG
  printf ("After qrsolv dxnorm = %g, delta = %g, fp = %g\n", dxnorm, delta, fp);
  printf ("sdiag = ") ; gsl_vector_fprintf(stdout, sdiag, "%g"); printf("\n");
  printf ("x = ") ; gsl_vector_fprintf(stdout, x, "%g"); printf("\n");
  printf ("r = ") ; gsl_matrix_fprintf(stdout, r, "%g"); printf("\nXXX\n");
#endif

  /* If the function is small enough, accept the current value of par */

  if (fabs (fp) <= 0.1 * delta)
    goto line220;

  if (par_lower == 0 && fp <= fp_old && fp_old < 0)
    goto line220;

  /* Check for maximum number of iterations */

  if (iter == 10)
    goto line220;

  /* Compute the Newton correction */

  compute_newton_correction (r, sdiag, perm, x, dxnorm, diag, w);

#ifdef DEBUG
  printf ("newton_correction = ");
  gsl_vector_fprintf(stdout, w, "%g"); printf("\n");
#endif

  {
    double wnorm = enorm (w);
    par_c = fp / (delta * wnorm * wnorm);
  }

#ifdef DEBUG
  printf("fp = %g\n", fp);
  printf("par_lower = %g\n", par_lower);
  printf("par_upper = %g\n", par_upper);
  printf("par_c = %g\n", par_c);
#endif

  /* Depending on the sign of the function, update par_lower or par_upper */

  if (fp > 0)
    {
      if (par > par_lower)
        {
          par_lower = par;
#ifdef DEBUG
      printf("fp > 0: set par_lower = par = %g\n", par);
#endif

        }
    }
  else if (fp < 0)
    {
      if (par < par_upper)
        {
#ifdef DEBUG
      printf("fp < 0: set par_upper = par = %g\n", par);
#endif
          par_upper = par;
        }
    }

  /* Compute an improved estimate for par */

#ifdef DEBUG
      printf("improved estimate par = MAX(%g, %g) \n", par_lower, par+par_c);
#endif

  par = GSL_MAX_DBL (par_lower, par + par_c);

#ifdef DEBUG
      printf("improved estimate par = %g \n", par);
#endif

  goto iteration;

line220:

#ifdef DEBUG
  printf("LEAVING lmpar, par = %g\n", par);
#endif

  *par_inout = par;

  return GSL_SUCCESS;
}