/*
 *  cpldosc_computefold.c
 *
 *  This code computes fold curves of the critical manifold
 *  of the coupled oscillator system.
 */

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_roots.h>


double cpldosc_foldfunc(double* x_, double* p_);
int cpldosc_foldfunc_grad(double* x, double* p, double* grad);
int cpldosc_ghat(double* x_, double* p_, double* ghat);
int cpldosc_dfhat(double* x_, double* p_, double df[][2]);


double ff_s(double s, void* par)
    {
    double v[2];
    double p[3];

    double sigma1 = gsl_vector_get(par,0);
    double sigma2 = gsl_vector_get(par,1);
    double omega  = gsl_vector_get(par,2);
    
    double v1 = gsl_vector_get(par,3);
    double v2 = gsl_vector_get(par,4);
    double a1 = gsl_vector_get(par,5);
    double a2 = gsl_vector_get(par,6);

    v[0] = v1 + s*a1;
    v[1] = v2 + s*a2;

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

    double det = cpldosc_foldfunc(v,p);

    return(det);
    }


double dff_s(double s, void* par)
    {
    double v[2];
    double p[3];
    double grad[2];

    double sigma1 = gsl_vector_get(par,0);
    double sigma2 = gsl_vector_get(par,1);
    double omega  = gsl_vector_get(par,2);
    
    double v1 = gsl_vector_get(par,3);
    double v2 = gsl_vector_get(par,4);
    double a1 = gsl_vector_get(par,5);
    double a2 = gsl_vector_get(par,6);

    v[0] = v1 + s*a1;
    v[1] = v2 + s*a2;

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

    cpldosc_foldfunc_grad(v,p,grad);
    double df = grad[0]*a1 + grad[1]*a2;

    return(df);
    }


void ffdff_s(double s, void *par, double *f, double *df)
    {
    *f = ff_s(s,par);
    *df = dff_s(s,par);
    }


double findfoldpoint(double v1, double v2, double a1, double a2, double sigma1, double sigma2, double omega)
    {
    double s, s0;
    double det;

    gsl_vector *par = gsl_vector_alloc(7);

    gsl_vector_set(par,0, sigma1);
    gsl_vector_set(par,1, sigma2);
    gsl_vector_set(par,2, omega);
    /* v is the base point. */
    gsl_vector_set(par,3, v1);
    gsl_vector_set(par,4, v2);
    /* a is the direction. */
    gsl_vector_set(par,5, a1);
    gsl_vector_set(par,6, a2);

    int status;
    int iter = 0, max_iter = 100;
    const gsl_root_fdfsolver_type *T;
    gsl_root_fdfsolver *solver;
    gsl_function_fdf FDF;
    FDF.f = &ff_s;
    FDF.df = &dff_s;
    FDF.fdf = &ffdff_s;
    FDF.params = par;

    T = gsl_root_fdfsolver_steffenson;
    solver = gsl_root_fdfsolver_alloc(T);
    s = 0.0;
    gsl_root_fdfsolver_set(solver,&FDF,s);

    do
        {
        ++iter;
        status = gsl_root_fdfsolver_iterate(solver);
        s0 = s;
        s = gsl_root_fdfsolver_root(solver);
        status = gsl_root_test_delta(s,s0,1e-13,1e-10);
        }
    while (status == GSL_CONTINUE && iter < max_iter);

    gsl_root_fdfsolver_free(solver);
    gsl_vector_free(par);

    return s;
    }


/*
 *  foldedeq_func computes a value that is zero if the fold point is
 *  a folded equilibrium.
 */

double foldedeq_func(double v1, double v2, double sigma1, double sigma2, double omega)
    {
    double ghat[2];
    double v[2];
    double p[3];
    double dfhat[2][2];

    v[0] = v1;
    v[1] = v2;
    p[0] = sigma1;
    p[1] = sigma2;
    p[2] = omega;

    cpldosc_ghat(v,p,ghat);
    cpldosc_dfhat(v,p,dfhat);

    double df11 = dfhat[0][0];
    double df21 = dfhat[1][0];
    double m = sqrt(df11*df11+df21*df21);
    double func = (-df21/m)*ghat[0] + (df11/m)*ghat[1];

    return(func);
    }


int main(int argc, char **argv)
    {
    int i, N;
    double s;
    double sigma1, sigma2, omega;
    double delta;
    double feq;
    double vv[2];
    double par[3];
    double ff_grad[2];

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

    /*
     *  Get the arguments given on the command line.
     */
    vv[0]   = atof(argv[1]);
    vv[1]   = atof(argv[2]);
    sigma1  = atof(argv[3]);
    sigma2  = atof(argv[4]);
    omega   = atof(argv[5]);
    delta   = atof(argv[6]);
    N       = atoi(argv[7]);

    fprintf(stderr,"v1=%f v2=%f sigma1=%f  sigma2=%f omega=%f\n",
                 vv[0],vv[1],sigma1,sigma2,omega);

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

    cpldosc_foldfunc_grad(vv,par,ff_grad); 

    s = findfoldpoint(vv[0],vv[1],ff_grad[0],ff_grad[1],sigma1,sigma2,omega);
    vv[0] = vv[0] + s*ff_grad[0];
    vv[1] = vv[1] + s*ff_grad[1];
    feq = foldedeq_func(vv[0],vv[1],sigma1,sigma2,omega);

    printf("%5d %18.15e %18.15e %18.15e\n",0,vv[0],vv[1],feq);

    for (i = 1; i <= N; ++i)
        {
        double beta;
        cpldosc_foldfunc_grad(vv,par,ff_grad);
        double mag = sqrt(ff_grad[0]*ff_grad[0]+ff_grad[1]*ff_grad[1]);
        if (mag > 1.0)
            beta = 1.0;
        else
            beta = mag;
        fprintf(stderr,"beta = %18.15e\n",beta);
        vv[0] = vv[0] + beta*delta*ff_grad[1]/mag;
        vv[1] = vv[1] - beta*delta*ff_grad[0]/mag;
        cpldosc_foldfunc_grad(vv,par,ff_grad);
        s = findfoldpoint(vv[0],vv[1],ff_grad[0],ff_grad[1],sigma1,sigma2,omega);
        vv[0] = vv[0] + s*ff_grad[0];
        vv[1] = vv[1] + s*ff_grad[1];
        feq = foldedeq_func(vv[0],vv[1],sigma1,sigma2,omega);
        printf("%5d %18.15e %18.15e %18.15e\n",i,vv[0],vv[1],feq);
        }

    return 0;
    }