#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

double findroot(double t0, double t1, double a, double b, double c, double d);
int createCubic(double h, double t0, double t1, double var0, double var1, double var0p, double var1p, double *a, double *b, double *c, double *d);

int main (int argc, const char * argv[])
{
    int i, below_zero, count, steps, failures;
    double x0, x1;
    double y0, y1;
    double u0, u1;
    double v0, v1;
    double t0, t1;
    double h;  /* timestep */
    double root; /* root of polynomial */
    double a, b, c, d; /* coefficients of polynomial */
    double y, u, v;
    double E_b, K_e, U_e, s;
    FILE *fp;
    char *str;
    const char *name;

    below_zero = 0;
    count = 0;
    
    if (argc < 4)
    {
        fprintf(stderr, "usage: ./with-scaling energy stepsize steps file\n");
        exit(1);
    }
    else
    {
        x1 = sqrt(2 * strtod(argv[1], (char **)NULL));
        h  = strtod(argv[2], (char **)NULL);
        steps = atoi(argv[3]);
	name = argv[4];
    }
    
    y1 = u1 = v1 = 0.0;
    
    fp = fopen(name, "w");
    str = malloc(80);
        
    /* find energy at beginning */
    E_b = (0.5) * (pow(u1, 2) + pow(v1, 2))
        + (0.5) * (pow(x1, 2) + pow(y1, 2) + (2.0 * pow(x1, 2) * y1) - ((2.0/3.0) * pow(y1, 3)));
    failures = 0;
    
    for (i = 1; i <= steps; i++)
    {
        /* half kick - drift - half kick leapfrog step of HH */
        u1 = u1 + ((h/2) * (-x1 - (2.0 * x1 * y1)));
        v1 = v1 + ((h/2) * (-y1 - pow(x1,2) + pow(y1,2)));
        x1 = x1 + (h * u1);
        y1 = y1 + (h * v1);
        u1 = u1 + ((h/2) * (-x1 - (2.0 * x1 * y1)));
        v1 = v1 + ((h/2) * (-y1 - pow(x1,2) + pow(y1,2)));
        
        /* find energy at end of step */
        K_e = (0.5) * (pow(u1, 2) + pow(v1, 2));
        U_e = (0.5) * (pow(x1, 2) + pow(y1, 2) + (2.0 * pow(x1, 2) * y1) - ((2.0/3.0) * pow(y1, 3)));
        
        /* find rescaling factor */
        if (E_b < U_e) {
           s = 0;
           failures++;
        }
        else  s = sqrt((E_b - U_e) / K_e );
        
        /* apply rescaling factor to velocities */
        u1 *= s;
        v1 *= s;

        if (below_zero)
        {
            if (x1 >= 0.0)
            {
               
                t1 = h * ((float)i);
                t0 = t1 - h;
                
                createCubic(h, t0, t1, x0, x1, u0, u1, &a, &b, &c, &d);
                root = findroot(t0, t1, a, b, c, d);

                /* now we have the time at which the x plane was crossed.
                 * need to create polys for other 3 variables and evaluate
                 * at root
                 */
                
                createCubic(h, t0, t1, y0, y1, v0, v1, &a, &b, &c, &d);
                y = ((a * pow(root, 3)) + (b * pow(root, 2)) + (c * root) + d);
                
                createCubic(h, t0, t1, u0, u1, (-x0 - (2 * x0 * y0)), (-x1 - (2 * x1 * y1)), &a, &b, &c, &d);
                u = ((a * pow(root, 3)) + (b * pow(root, 2)) + (c * root) + d);
                
                createCubic(h, t0, t1, v0, v1, (-y0 - pow(x0,2) + pow(y0,2)), (-y1 - pow(x1,2) + pow(y1,2)), &a, &b, &c, &d);
                v = ((a * pow(root, 3)) + (b * pow(root, 2)) + (c * root) + d);

                sprintf(str, "%f   %f\n", y, v);
                fwrite(str, strlen(str), 1, fp);
                
                count++;
                below_zero = 0;
            }
            else
            {
                x0 = x1;
                y0 = y1;
                u0 = u1;
                v0 = v1;
            }
        }
        else
        {
            if (x1 < 0.0)
            {
                below_zero = 1;
            }
        }
    }
    
    free(str);
    close(fp);
        
    printf("%d\n", failures);
    return 0;
}

double findroot(double t0, double t1, double a, double b, double c, double d)
{
    double tbar, value;
    
    /* we now have our polynomial, use bisection to determine root
     * we know a priori that f(t0) < 0, and f(t1) > 0
     */
    while ((t1 - fabs(t0)) > 0.0000000001)  
    {
        tbar = (t1 + t0) / 2.0;
        value = ((a * pow(tbar, 3)) + (b * pow(tbar, 2)) + (c * tbar) + d);
        if (value > 0.0)
        {
           t1 = tbar;
        }
        else if (value < 0.0)
        {
            t0 = tbar;
        }
        else
        {
            t0 = t1 = tbar;
        }
    } /* use tbar as your root. they're all 3 within epsilon, so 
       * it shouldn't matter which
       */
    return tbar;
}

/* takes in h - timestep
 *          step - number of steps so far
 *          var0, var1 - the variable at the two timesteps
 *          var0p, var1p - the derivate of said variable at same timesteps
 *          *a, *b, *c, *d - coefficients of cubic returned - a is x^3 coeff, etc.
 */
int createCubic(double h, double t0, double t1, double var0, double var1, double var0p, double var1p, double *a, double *b, double *c, double *d)
{
    double c0, c1, c2, c3;
    
    c0 = var0;
    c1 = ((var1 - var0) / h);
    c2 = ((c1 - var0p) / h);
    c3 = ((var1p - c1 - (c2 * h)) / pow(h,2));

    (*a) = c3;
    (*b) = (c2 - (2 * c3 * t0) - (c3 * t1));
    (*c) = (c1 - (c2 * (t0 + t1)) + (c3 * pow(t0, 2)) + (2 * c3 * t0 * t1));
    (*d) = (c0 - (c1 * t0) + (c2 * t0 * t1) - (c3 * pow(t0, 2) * t1));

    return 0;
}