#include <iostream>
#include <cmath>
using namespace std;

#define real double

#define XMIN	0.0
#define XMAX	1.0
#define ALPHA	1.0
#define BETA	2.0
#define W	200.0

#define N	2
#define TOL	0.01	// large tolerance because we will use secant at end

#define GRAPHICS_OUTPUT	1	 // handy way to turn graphics output on/off

// Midpoint/RK4 method y(x), modular form, array version.

void get_dydx(real y[], real x, real dydx[])	// specify the ODE.
{
    dydx[0] = y[1];
    dydx[1] = -W*y[0];
}

void midpoint_step(real y[], real& x, real dx)	// Midpoint integrator
{
    int i;
    real dydx[N], dy[N], y1[N];

    get_dydx(y, x, dydx);

    for (i = 0; i < N; i++) {
	dy[i] = dx*dydx[i];
	y1[i] = y[i] + 0.5*dy[i];
    }

    get_dydx(y1, x+0.5*dx, dydx);

    for (i = 0; i < N; i++) {
	dy[i] = dx*dydx[i];
	y[i] += dy[i];
    }

    x += dx;
}

void rk4_step(real y[], real& x, real dx)	// Runge-Kutta-4 integrator
{
    int i;
    real dydx[N], dy1[N], dy2[N], dy3[N], dy4[N],
		  y1[N], y2[N], y3[N], y4[N];

    get_dydx(y, x, dydx);

    for (i = 0; i < N; i++) {
	dy1[i] = dx*dydx[i];
	y1[i] = y[i] + 0.5*dy1[i];
    }

    get_dydx(y1, x+0.5*dx, dydx);

    for (i = 0; i < N; i++) {
	dy2[i] = dx*dydx[i];
	y2[i] = y[i] + 0.5*dy2[i];
    }

    get_dydx(y2, x+0.5*dx, dydx);

    for (i = 0; i < N; i++) {
	dy3[i] = dx*dydx[i];
	y3[i] = y[i] + dy3[i];
    }

    get_dydx(y3, x+dx, dydx);

    for (i = 0; i < N; i++) {
	dy4[i] = dx*dydx[i];
	y[i] += (dy1[i] + 2*dy2[i] + 2*dy3[i] + dy4[i])/6.0;
    }

    x += dx;
}

bool time_to_stop(real y[], real x, real x_max)
{
    return (x >= x_max);
}

void output(real y[], real x)		// output to stdout
{
    if (GRAPHICS_OUTPUT) {

        int i;

	printf("%f ", x);
	for (i = 0; i < N; i++)
	  printf("%f ", y[i]);
	printf("\n");
    }
}

void initialize(real y[], real& x, real& dx, real& x_max, real z)
{
    // Specify initial conditions.

    x = XMIN;

    y[0] = ALPHA;
    y[1] = z;				// guess of initial slope

    dx = .001;				// conservatively short step
    x_max = XMAX;
}

// Bisection method for g(z) = 0.

real g(real z)				// the equation to solve involves  
{					// solving an initial-value problem
    real y[N], x, dx, x_max;

    initialize(y, x, dx, x_max, z);

    while (!time_to_stop(y, x, x_max)) {
	output(y, x);
	rk4_step(y, x, dx);
    }

    output(y, x);

    return y[0] - BETA;
}

int find_root(real& zl, real& zr)	// bisection to tolerance TOL
{
    int n = 0;
    while (fabs(zl - zr) > TOL) {
	real zm = 0.5*(zl + zr);
	if (g(zm)*g(zl) > 0)
	    zl = zm;
	else
	    zr = zm;
	n++;
	fprintf(stderr, "%d: zl = %f, g = %f, zr = %f, g = %f\n",
		n, zl, g(zl), zr, g(zr));
    }
    return n;
}

real secant(real& zl, real& zr)		// one secant iteration
{
    real zs = zl + (zr - zl) * (-g(zl)) / (g(zr) - g(zl));

    if (g(zs)*g(zl) > 0)
	zl = zs;
    else
	zr = zs;

    return zs;
}

#define FAC 1.5

void bracket_root(real& zl, real& zr)
{
    int id = 0;

    zl = -0.5;				// initial range
    zr = -zl;

    // Expand the range by alternately increasing zl and zr by a
    // factor FAC.  Note that this is only guaranteed to work if
    // the system is known to have a SINGLE root.

    while (g(zl)*g(zr) > 0) {
        zl *= FAC;
	id = 1;
	if (g(zl)*g(zr) > 0) {
	  zr *= FAC;
	  id = 2;
	}
    }

    // Now we know which increase was successful in bracketing
    // the root: id = 1 <==> zl; id = 2 <==> zr.

    if (id == 1) 
        zr = (zl)/FAC;
    else
        zl = (zr)/FAC;
      
}

main()
{
    real zl, zr;
    int n;

    // "-C" commands are to set color in plot_data.

    if (GRAPHICS_OUTPUT) printf("-C green\n");
    bracket_root(zl, zr);

    fprintf(stderr, "0: zl = %f, g = %f, zr = %f, g = %f\n",
	    zl, g(zl), zr, g(zr));

    if (GRAPHICS_OUTPUT) printf("-C blue\n");
    n = find_root(zl, zr);

    fprintf(stderr, "Root lies in range (%f, %f) after %d iterations.\n",
	    zl, zr, n);
    fprintf(stderr, "Function value at midpoint = %g\n",
	    g(0.5*(zl+zr)));

    // Refine the root using the secant method.

    if (GRAPHICS_OUTPUT) printf("-C red\n");
    fprintf(stderr, "Function value after 1 secant iteration  = %g\n",
	    g(secant(zl, zr)));
    fprintf(stderr, "Function value after 2 secant iterations = %g\n",
	    g(secant(zl, zr)));
}