#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_odeiv.h>


/*
 *  gradient
 *
 *  Compute the gradient of the event function using finite differences.
 */
static void gradient(double (*func)(const double *y, const double *p),
                      const int n, double *y0, const double *p, double *grad)
    {
    const double tiny = 1.0e-8;
    double g0;
    int i;

    g0 = (*func)(y0,p);
    for (i = 0; i < n; ++i)
        {
        double g1, tmp;
        tmp = y0[i];
        y0[i] = y0[i]+tiny;
        g1 = (*func)(y0,p);
        grad[i] = (g1-g0)/tiny;
        y0[i] = tmp;
        }
    return;
    }

/*
 *  evt_stepsize
 *
 *  Computes a maximum step size, based on the derivative of the event function
 *  along the vector field.
 *
 *  Arguments:
 *     sys            Pointer to the gsl_odeiv_system object that defines the
 *                    vector field.
 *     y              State vector
 *     hmin, hmax     The minimum and maximum step sizes allowed.
 *     eventfunc      The event function
 *     grad_eventfunc The gradient of the event function.  If this is NULL,
 *                    the gradient will be approximated numerically.
 *     f              The vector field at y.  If this is NULL, the vector field
 *                    function in sys will be called to compute f.
 *
 *  The function returns a step size; if the return value is negative,
 *  it means that a call to malloc failed.
 */

double evt_stepsize(const gsl_odeiv_system *sys, double *y,
                    const double hmin, const double hmax,
                    double (*eventfunc)(const double *y, const double *params),
                    void (*grad_eventfunc)(const double *y, const double *params,
                                                              const double *grad),
                    double *f )
    {
    int i;
    double g;
    double h;
    const int n = sys->dimension;
    double *mf;
    double *grad = (double *) malloc(n*sizeof(double));
    if (grad == NULL)
        {
        return -1.0;  /* malloc failed */
        }

    /* Compute the gradient of the event function */
    if (grad_eventfunc == NULL)
        {
        gradient(eventfunc,n,y,sys->params,grad);
        }
    else
        {
        (*grad_eventfunc)(y,sys->params,grad);
        }

    /* Compute the vector field at y if it wasn't given as an argument. */
    if (f == NULL)
        {
        mf = (double *) malloc(n*sizeof(double));
        if (mf == NULL)
            {
            free(grad);
            return -1.0;  /* malloc failed */
            }
        GSL_ODEIV_FN_EVAL(sys,0.0,y,mf);
        }
    else
        {
        mf = f;
        }

    /*
     *  Compute the dot product of the gradient of the event function and
     *  the vector field.
     */
    double dot = 0.0;
    for (i = 0; i < n; ++i)
        {
        dot = dot + grad[i]*mf[i];
        }
    free(grad);
    if (f == NULL)
        {
        free(mf);
        }
    /* Compute the event function at y */
    g = (*eventfunc)(y,sys->params);
    if (dot == 0.0)
        /* Not likely, but just in case... */
        {
        h = hmax;
        }
    else
        {
        const double S = 0.5;   /* "Safety factor" makes the step smaller than */ 
                                /* dictated by the formula.                    */
        h = S*fabs(g/dot);
        h = GSL_MIN_DBL(h,hmax);
        h = GSL_MAX_DBL(h,hmin);
        }
    return h;
    }