/*
 * cpldosc_slowvf_main.c
 *
 *  This file compute the solution of the slow flow, and stops
 *  when the solution reaches a fold.
 *  The initial conditions, parameter values (for sigma1, sigma2,
 *  and omega), and the stop time (in case it doesn't reach a fold)
 *  are given on the command line.
 */


#include <stdio.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_odeiv.h>
#include <math.h>

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,
                                                              double *grad),
                    double *f );

double cpldosc_foldfunc(double *x, double *p);
int cpldosc_foldfunc_grad(double *x, double *p, double *grad);
int cpldosc_fhat(double *x, double *p, double *q);
int cpldosc_slowvf(double t, const double x_[], double * vf_, void * params);
int cpldosc_slowvf_jac(double t, const double x_[], double *jac_, double *dfdt_, void *params);


int main (int argc, char *argv[])
    {
    double prevt, y[2], prevy[2];
    double q[2];

    double p[3];

    if (argc != 7)
        {
        fprintf(stderr,"use: %s v1 v2 sigma1 sigma2 omega time_duration\n",argv[0]);
        exit(-1);
        }

    /*
     *  Get the values given on the command line.
     */
    double v1     = atof(argv[1]);
    double v2     = atof(argv[2]);
    double sigma1 = atof(argv[3]);
    double sigma2 = atof(argv[4]);
    double omega  = atof(argv[5]);
    double t1     = atof(argv[6]);

    /*
     *  Set up the GSL ODE solver.
     */
    const gsl_odeiv_step_type * T  = gsl_odeiv_step_rk8pd;
    gsl_odeiv_step * step    = gsl_odeiv_step_alloc(T, 2);
    gsl_odeiv_control * control = gsl_odeiv_control_y_new(1e-12, 0.0);
    gsl_odeiv_evolve * evolve  = gsl_odeiv_evolve_alloc(2);

    p[0]  =  sigma1;
    p[1]  =  sigma2;
    p[2]  =  omega;

    y[0] = v1;
    y[1] = v2;

    double hmax = 0.1;
    double hmin = 1e-6;

    gsl_odeiv_system sys = {cpldosc_slowvf, cpldosc_slowvf_jac, 2, &(p[0])};

    double t  = 0.0;

    double g0 = cpldosc_foldfunc(y,p);

    double ztol = 1.0e-12;
    int dir = 0;  /* 1=increasing, -1=decreasing */

    double h = hmax;
    while (t < t1)
        {
        prevt    = t;
        prevy[0] = y[0];
        prevy[1] = y[1];

        cpldosc_fhat(y,p,q);
        printf("%16.12e %16.12e %16.12e %16.12e %16.12e %16.12e\n", t, y[0], y[1], q[0],q[1],cpldosc_foldfunc(y,p));

        /*
         *  Compute the step size.
         */
        double hevt = evt_stepsize(&sys,y,hmin,hmax,cpldosc_foldfunc,cpldosc_foldfunc_grad,NULL);
        if (hevt < 0.0)
            {
            fprintf(stderr,"evt_stepsize returned %f\n",hevt);
            break;
            }
        if (h > hevt)
            {
            h = hevt;
            }
        /*
         *  Advance the solution by a step.
         */
        int status = gsl_odeiv_evolve_apply(evolve, control, step, &sys, &t, t1, &h, y);
        if (status != GSL_SUCCESS)
            {
            fprintf(stderr,"status=%d\n",status);
            break;
            }
        /*
         *  Check for a change of sign of the event function, and handle it.
         */
        double g1 = cpldosc_foldfunc(y,p);

        if (g1*g0 <= 0.0)
            {
            /* The sign of the event function changed. */
            if (g1 == 0.0 & (dir == 0 | (g0 > 0 & dir == -1) | (g0 < 0 & dir == 1)) )
                {
                /* Remarkably, the solution landed exactly on the event boundary! */
                break;
                }
            else if  ((dir == 0) | (g1 < 0.0 & dir == -1) | (g1 > 0.0 & dir == 1))
                {
                /*
                 *  Use the ODE step function and the method of regula falsi
                 *  to find the event.
                 */
                double prevg = cpldosc_foldfunc(prevy,p);
                double m;
                /* Recalculate h, since gsl...apply may have changed it.*/
                h = t - prevt; 
                double dydt_in[2];
                double dydt_out[2];
                GSL_ODEIV_FN_EVAL(&sys,t,y,dydt_in);
                int maxiters = 10;
                while ((fabs(g1) > ztol) & (maxiters > 0))
                    {
                    double y_err[2];

                    --maxiters;
                    m = g1/(prevg-g1);
                    h = m*h;
                    fprintf(stderr,"h=%20.16f  maxiters=%d\n",h,maxiters);
                    prevy[0] = y[0];
                    prevy[1] = y[1];
                    prevg    = g1;
                    int status = gsl_odeiv_step_apply(step,t,h,y,y_err,dydt_in,dydt_out,&sys);
                    if (status != GSL_SUCCESS)
                        {
                        fprintf(stderr,"gsl_odeiv_step_apply returned an error while refining the event location.\n");
                        break;
                        }
                    dydt_in[0] = dydt_out[0];
                    dydt_in[1] = dydt_out[1];
                    g1 = cpldosc_foldfunc(y,p);
                    }
                if (maxiters == 0)
                    {
                    fprintf(stderr,"regula falsi did not converge; after maxiters steps, we have:\n");
                    }
                fprintf(stderr,"v1=%20.16f  v2=%20.16f  g=%20.16e (regula falsi)\n",
                                 y[0],y[1],cpldosc_foldfunc(y,p));
                cpldosc_fhat(y,p,q);
                printf("%16.12e %16.12e %16.12e %16.12e %16.12e %16.12e\n", t, y[0], y[1], q[0],q[1],cpldosc_foldfunc(y,p));
                break;
                }
            else
                {
                /* Wrong direction, don't stop here. */
                g0 = g1;
                }
            }
        }

    gsl_odeiv_evolve_free(evolve);
    gsl_odeiv_control_free(control);
    gsl_odeiv_step_free(step);
    return 0;
    }