/*
# Copyright (c) 2005, 2007, 2008 Andrew Fraser
# This file is part of HMM_DS_Code.
#
# HMM_DS_Code 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.
#
# HMM_DS_Code is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Public License for more details.
#
# You should have received a copy of the GNU General Public License along with
# this program; if not, write to the Free Software Foundation, Inc.,
# 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
#

// ToDo: Run 'indent'.
// ToDo: Maybe rename some variables to more descriptive longer names.
// ToDo: Mark all functions not exported as 'static'.
// ToDo: Markup source for 'doxygen'.
// ToDo: ? Is thread safety an issue?  Would it be useful to make this
// ToDo:   code thread-safe ?

*/

#include <stdio.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_poly.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_roots.h>

struct ParPac {
  double s,b,r;
};

/* A function that calls a gsl integrator to step the Lorenz system
   forward in time a spcified number of steps.  The Lorenz system is:

   \dot x = s(y-x)
   \dot y = rx -xz -y
   \dot z = xy -bz

   Derived from http://sources.redhat.com/gsl/ref/gsl-ref_25.html
   also see http://www.gnu.org/software/gsl/manual/gsl-ref_toc.html
*/

int Lfunc (double t, const double x[3], double f[3],
      void *params)
{
  struct ParPac *pac = (struct ParPac *) params;
  double s = pac->s;
  double b = pac->b;
  double r = pac->r;

  /* These three equations are the vector field for the Lorenz system */
  f[0] = (s*(x[1] - x[0]));
  f[1] = (x[0]*(r-x[2]) - x[1]);
  f[2] = (x[0]*x[1] - b*x[2]);

  return GSL_SUCCESS;
}

/* Ljac may be out of date.  Check before using it */

int
Ljac (double t, const double x[], double *dfdx, 
     double dfdt[], void *params)
{
  double *dpp = (double *) params;
  double s = dpp[0];
  double b = dpp[1];
  double r = dpp[2];
  gsl_matrix_view dfdx_mat 
    = gsl_matrix_view_array (dfdx, 3, 3);
  gsl_matrix * m = &dfdx_mat.matrix;
  /*               out, in */
  gsl_matrix_set (m, 0, 0, -s);
  gsl_matrix_set (m, 0, 1, s);
  gsl_matrix_set (m, 0, 2, 0); 
  gsl_matrix_set (m, 1, 0, r);
  gsl_matrix_set (m, 1, 1, -1);
  gsl_matrix_set (m, 1, 2, -x[0]); 
  gsl_matrix_set (m, 2, 0, x[1]);
  gsl_matrix_set (m, 2, 1, x[0]);
  gsl_matrix_set (m, 2, 2, -b);
  dfdt[0] = 0.0;
  dfdt[1] = 0.0;
  dfdt[2] = 0.0;
  return GSL_SUCCESS;
}

int Lsteps(
    double *y,          /* Initial condition      y[0:2]         */
    struct ParPac *pac, /* Paramters                             */
    double ts,          /* Time step                             */
    int Nt,             /* Number of time steps                  */
    float *result)      /* Memory for results                    */
{
  const gsl_odeiv_step_type * T = gsl_odeiv_step_rkf45;
  gsl_odeiv_step * s = gsl_odeiv_step_alloc (T, 3);
/*This function returns a pointer to a newly allocated instance of a
  stepping function of type T for a system of 3 dimensions. */

  gsl_odeiv_control * c = gsl_odeiv_control_y_new (1e-7, 0.0);
/*This function creates a new control object which will keep the local
  error on each step within an absolute error of 1e-7.  If the second
  argument were eps_rel and not zero, it would keep the relative error
  of eps_rel with respect to the solution y_i(t). */

  gsl_odeiv_evolve * e = gsl_odeiv_evolve_alloc (3);
/* This function returns a pointer to a newly allocated instance of an
   evolution function for a system of 3 dimensions. */

  gsl_odeiv_system sys = {Lfunc, Ljac, 3, pac};
  double t, t1;
  double h = 1e-6;
  int i;

  result[0] = y[0];
  result[1] = y[1];
  result[2] = y[2];
  
  t = 0;
  for(i=1;i<Nt;i++)
  {
    t1 = i*ts;
    while (t < t1)
    {
	int status = gsl_odeiv_evolve_apply (e, c, s, &sys, &t, t1, &h, y);
	/* This function advances the system (e, sys) from time t and
	   position y using the stepping function s. The new time and
	   position are stored in t and y on output. The initial
	   step-size is taken as h, but this will be modified using
	   the control function c to achieve the appropriate error
	   bound if necessary. The routine may make several calls to s
	   in order to determine the optimum step-size. If the
	   step-size has been changed the value of h will be modified
	   on output. The maximum time t1 is guaranteed not to be
	   exceeded by the time-step. On the final time-step the value
	   of t will be set to t1 exactly. */
	if (status != GSL_SUCCESS)
	{
	  fprintf(stderr,
		  "failure of gsl_odeiv_evolve_apply at time step %d.\n",i);
	  return(-1);
	}
    }
    result[3*i]   = y[0];
    result[3*i+1] = y[1];
    result[3*i+2] = y[2];
  }
  gsl_odeiv_evolve_free(e);
  gsl_odeiv_control_free(c);
  gsl_odeiv_step_free(s);
  return(0);
}

/*##################################################################*/
/*  The code below is like that above but it integrates the tangent */
/*
   \dot X = F(X)    With components:

   \dot x_0 = s(x_1-x_0)
   \dot x_1 = rx_0 -x_0x_2 -x_1
   \dot x_2 = x_0x_1 -bx_2

   Solution: X(t) = \phi(X(0),t)

   T(t) \equiv \frac{partial x(t)}{\partial x(0)}

   \dot T = DF T

   DF = [ -s        s   0   ]
        [ (r-x_2)  -1   -x_0]
        [ x_1      x_0  -b  ]
*/

int Ltan (double t, const double x[12], double f[12],
      void *params) 
{
  struct ParPac *pac = (struct ParPac *) params;
  double s = pac->s;
  double b = pac->b;
  double r = pac->r;
  double DF[3][3] = {-s,      s,    0,     /* This is the derivative */
		     r-x[2], -1, -x[0],	   /*   of the vector field  */
		     x[1],  x[0], -b};
  double *T[3];
  double *Tdot[3];
  {int i,j,k;                              /* Set T and Tdot to point */
  for(i = 0; i<3; i++){                    /* into the arguments x    */
    T[i]    = x + 3*(i+1);                 /* and f respectively      */
    Tdot[i] = f + 3*(i+1);
  }

  /* These three equations are the vector field for the Lorenz system */
  f[0] = (s*(x[1] - x[0]));
  f[1] = (x[0]*(r-x[2]) - x[1]);
  f[2] = (x[0]*x[1] - b*x[2]);
  /* Set Tdot = DF * T */
  for(i = 0; i<3; i++){
    for(j = 0; j<3; j++){
      Tdot[i][j] = 0;
      for(k = 0; k<3; k++){
	Tdot[i][j] += DF[i][k] * T[k][j];
      }
    }
  }
  }
  return GSL_SUCCESS;
}

int Ltansteps(
    double *y,          /* Initial condition      y[0:11]         */
    struct ParPac *pac, /* Paramters                             */
    double ts,          /* Time step                             */
    int Nt,             /* Number of time steps                  */
    double *result)     /* Want result[Nt][12] but weave can't   */
{
  const gsl_odeiv_step_type * T = gsl_odeiv_step_rkf45;
  gsl_odeiv_step * s = gsl_odeiv_step_alloc (T, 12);
/*This function returns a pointer to a newly allocated instance of a
  stepping function of type T for a system of 12 dimensions. */

  gsl_odeiv_control * c = gsl_odeiv_control_y_new (1e-7, 0.0);
/*This function creates a new control object which will keep the local
  error on each step within an absolute error of 1e-7.  If the second
  argument were eps_rel and not zero, it would keep the relative error
  of eps_rel with respect to the solution y_i(t). */

  gsl_odeiv_evolve * e = gsl_odeiv_evolve_alloc (12);
/* This function returns a pointer to a newly allocated instance of an
   evolution function for a system of 12 dimensions. */

  gsl_odeiv_system sys = {Ltan, NULL, 12, pac};
  double t, t1;
  double h = 1e-6;
  int i,it;

  for(i=0;i<12;i++)
    result[i] = y[i];
  t = 0;
  for(it=1;it<Nt;it++){           /* Loop over time steps */
    /* Set T to identity */
    int j;
    for(j=3;j<12;j++) y[j] = 0;
    y[3] = y[7] = y[11] = 1.0;

    t1 = it*ts;
    /* This while loop integrates to time t1 */
    while (t < t1){
      int status = gsl_odeiv_evolve_apply (e, c, s, &sys, &t, t1, &h, y);
      if (status != GSL_SUCCESS){
	fprintf(stderr,
		"failure of gsl_odeiv_evolve_apply at time step %d.\n",it);
	return(-1);
      }
    } /* End of Integration loop */
    for(j=0;j<12;j++)
      result[it*12+j] = y[j];
  } /* End of loop over time steps */
  gsl_odeiv_evolve_free(e);
  gsl_odeiv_control_free(c);
  gsl_odeiv_step_free(s);
  return(0);
}

/* this simply integrates backwards in time.  For comments see Lsteps above */

int LBfunc (double t, const double x[3], double f[3],
      void *params)
{
  struct ParPac *pac = (struct ParPac *) params;
  double s = pac->s;
  double b = pac->b;
  double r = pac->r;

  /* These three equations are the vector field for the Lorenz system */
  f[0] = -(s*(x[1] - x[0]));
  f[1] = -(x[0]*(r-x[2]) - x[1]);
  f[2] = -(x[0]*x[1] - b*x[2]);

  return GSL_SUCCESS;
}

int LBstep(
	   double *y,          /* Initial condition      y[0:2]         */
	   struct ParPac *pac, /* Paramters                             */
	   double ts,          /* Time step                             */
	   float *result)      /* Array to hold results  result[0:2] */
{
  const gsl_odeiv_step_type * T = gsl_odeiv_step_rkf45;
  gsl_odeiv_step * s = gsl_odeiv_step_alloc (T, 3);
  gsl_odeiv_control * c = gsl_odeiv_control_y_new (1e-2, 0.0);
  gsl_odeiv_evolve * e = gsl_odeiv_evolve_alloc (3);
  gsl_odeiv_system sys = {LBfunc, Ljac, 3, pac};
  double t;
  double h = 1e-6;
  
  t = 0;
  while (t < ts)
    {
      int status = gsl_odeiv_evolve_apply (e, c, s, &sys, &t, ts, &h, y);
      if (status != GSL_SUCCESS)
	{
	  fprintf(stderr,
		  "failure of gsl_odeiv_evolve_apply.\n");
	  return(-1);
	}
    }
  result[0] = y[0];
  result[1] = y[1];
  result[2] = y[2];
  gsl_odeiv_evolve_free(e);
  gsl_odeiv_control_free(c);
  gsl_odeiv_step_free(s);
  return(0);
}