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

#define DEBUG

#define MIN(a, b) ((a) < (b) ? (a) : (b))

enum error_status { CONVERGED, INVALID_RANGE, NO_ROOT, NO_CONVERGENCE };

typedef struct result_s {
	unsigned int iterations;
	error_status error;
} result;

/*
Linear interpolation to x-axis based on tangent line using derivative

x_(n+1) = x_n - f(x_n) / f'(x_n)

Small f'(x) leads to poor or no convergence
*/
double newton_raphson(double(*f)(double), double(*f_deriv)(double), double x, double tol, unsigned int maxiter, result *r) {
	r->iterations = 0;
	r->error = CONVERGED;

	// Check if already given root
	if (f(x) == 0) {
		return x;
	}

	for (r->iterations = 1; r->iterations < maxiter; r->iterations++) {
		r->iterations++;
		double x_est = x - f(x) / f_deriv(x);

		if (f(x_est) == 0 || fabs(x_est - x) <= tol) {
			return x_est;
		}

		x = x_est;
	}

	r->error = NO_CONVERGENCE;
	return 0;
}

/*
Bisection method, use Intermediate Value Theorem over [a, b] to reduce range of root by half iteratively
Replace a or b with (a+b)/2 depending on which one matches sign until interval is sufficiently small

Slow convergence, but guaranteed to find a root in [a, b]
*/
double bisect(double(*f)(double), double a, double b, double tol, unsigned int maxiter, result *r) {
	r->iterations = 0;
	r->error = CONVERGED;

	double fa = f(a);
	double fb = f(b);

	// Check if already given root
	if (fa == 0) {
		return a;
	}
	if (fb == 0) {
		return b;
	}

	if (fa*fb > 0) {
		r->error = INVALID_RANGE;
		return 0;
	}

	for (r->iterations = 1; r->iterations < maxiter; r->iterations++) {
		double x_est = (b + a) / 2;
		double fx_est = f(x_est);

		if (fx_est * fa > 0) {
			a = x_est;
			fa = fx_est;
		}
		else {
			b = x_est;
			fb = fx_est;
		}

		if (fx_est == 0 || fabs(b - a) <= tol) {
			return x_est;
		}
	}

	r->error = NO_CONVERGENCE;
	return 0;
}

/*
Alternative to Newton-Raphson which uses an approximation of the derivative instead

Replace f'(x_n) = ( f(x_n) - f(x_(n-1)) ) / ( x_n - x_(n-1) )

x_(n+1) = x_n - f(x_n) * ( x_n - x_(n-1) ) / ( f(x_n) - f(x_(n-1)) )
*/
double secant(double(*f)(double), double x1, double x2, double tol, unsigned int maxiter, result *r) {
	r->iterations = 0;
	r->error = CONVERGED;

	double fx1 = f(x1), fx2 = f(x2);
	// Check if already given root
	if (fx1 == 0) {
		return x1;
	}
	if (fx2 == 0) {
		return x2;
	}

	// Set x1 to be closest to zero
	if (fabs(fx1) > fabs(fx2)) {
		double swap = x1;
		x1 = x2;
		x2 = swap;

		swap = fx1;
		fx1 = fx2;
		fx2 = swap;
	}

	for (r->iterations = 1; r->iterations < maxiter; r->iterations++) {
		double x_est = x1 - fx1 * (x1 - x2) / (fx1 - fx2);
		double fx_est = f(x_est);

		if (fx_est == 0 || fabs(x_est - x1) <= tol) {
			return x_est;
		}

		x2 = x1;
		fx2 = fx1;
		x1 = x_est;
		fx1 = fx_est;
	}

	r->error = NO_CONVERGENCE;
	return 0;
}

/*
Same as secant method, instead of using x_n_prev we use the result x_n estimate
to change the interval to [a, x_n], [x_n, b] depending on which retains the
interval changing sign

Slower convergence, but unlike secant method stays within the interval like the bisection method
*/
double false_position(double(*f)(double), double a, double b, double tol, unsigned int maxiter, result *r) {
	r->iterations = 0;
	r->error = CONVERGED;

	double fa = f(a);
	double fb = f(b);
	// Check if already given root
	if (fa == 0) {
		return a;
	}
	if (fb == 0) {
		return b;
	}

	if (fa*fb > 0) {
		r->error = INVALID_RANGE;
		return 0;
	}

	for (r->iterations = 1; r->iterations < maxiter; r->iterations++) {
		double x_est = (a * fb - b * fa) / (fb - fa);
		double fx_est = f(x_est);
		double del;

		if (fx_est * fa > 0) {
			del = x_est - a;
			a = x_est;
			fa = fx_est;
		}
		else {
			del = x_est - b;
			b = x_est;
			fb = fx_est;
		}

		if (fx_est == 0 || fabs(del) < tol) {
			return x_est;
		}
	}

	r->error = NO_CONVERGENCE;
	return 0;
}

/*
Ridders' Method is a variation on the false position method,
except instead of using the secant method for the interpolation instead the midpoint
in the interval is calculated and then an exponential factor is removed to linearize the residual
The false position is then applied on f(a), f(mid) e^Q, f(b) e^(2Q)

Converges super-linearly and is robust compared to other methods.
*/
double ridders(double(*f)(double), double a, double b, double tol, unsigned int maxiter, result *r) {
	r->iterations = 0;
	r->error = CONVERGED;

	double fa = f(a);
	double fb = f(b);
	// Check if already given root
	if (fa == 0) {
		return a;
	}
	if (fb == 0) {
		return b;
	}

	if (fa*fb > 0) {
		r->error = INVALID_RANGE;
		return 0;
	}

	for (r->iterations = 1; r->iterations < maxiter; r->iterations++) {
		double x_mid = (a + b) / 2;
		double fmid = f(x_mid);

		double x_est = x_mid;
		if (fa > fb) {
			x_est += (x_mid - a) * fmid / sqrt(fmid*fmid - fa * fb);
		}
		else {
			x_est -= (x_mid - a) * fmid / sqrt(fmid*fmid - fa * fb);
		}
		double fx_est = f(x_est);

		double del;
		if (fx_est * fa > 0) {
			del = a - x_est;
			a = x_est;
			fa = fx_est;
		}
		else {
			del = b - x_est;
			b = x_est;
			fb = fx_est;
		}

		if (fx_est == 0 || fabs(del) <= tol) {
			return x_est;
		}

	}

	r->error = NO_CONVERGENCE;
	return 0;
}

/*
Brent's Method, most common and robust 1D root finding method
Combination of inverse quadratic interpolation, secant method, and bisection method
The method used depends on which one lies within the interval
At worst it converges as fast as the bisection method, and at best quadratically.
*/
double brent(double(*f)(double), double a, double b, double tol, unsigned int maxiter, result *r) {
	r->iterations = 0;
	r->error = CONVERGED;

	double fa = f(a);
	double fb = f(b);
	// Check if already given root
	if (fa == 0) {
		return a;
	}
	if (fb == 0) {
		return b;
	}

	if (fa*fb > 0) {
		r->error = INVALID_RANGE;
		return 0;
	}

	if (fabs(fa) < fabs(fb)) {
		double swap = a;
		a = b;
		b = swap;

		swap = fa;
		fa = fb;
		fb = swap;
	}

	double c = a, fc = fa;
	double d = 99e99;
	bool mflag = true;

	for (r->iterations = 1; r->iterations < maxiter; r->iterations++) {
		double fc = f(c), s = 0;

		// quadratic inverse interpolation
		if ((fa != fc) && (fb != fc)) {
			s += a * fb * fc / (fa - fc) / (fa - fb);
			s += b * fc * fa / (fb - fa) / (fb - fc);
			s += c * fa * fb / (fc - fb) / (fc - fa);
		}
		// secant method
		else {
			s = b - fb * (b - a) / (fb - fa);
		}

		// Determine whether to use bisection step instead, Brent's main contribution to algorithm
		// Is ((3a + b) / 4 < s < b) or ((3a + b) / 4 > s > b) ? If no use bisection
		// Simplified using De Morgan's Laws
		if ((s <= ((3 * a + b) / 4) || s >= b) && (s >= ((3 * a + b) / 4) || s <= b)) {
			mflag = true;
			s = (a + b) / 2;
		}
		// test step if we used bisection last time
		else if (mflag && ((fabs(s - b) >= (fabs(b - c) / 2)) || (fabs(b - c) < fabs(tol)))) {
			mflag = true;
			s = (a + b) / 2;
		}
		// test step if we did not use bisection last time
		else if (!mflag && ((fabs(s - b) >= (fabs(c - d) / 2)) || (fabs(c - d) < fabs(tol)))) {
			mflag = true;
			s = (a + b) / 2;
		}
		// Continue to use quad interp or secant
		else {
			mflag = false;
		}

		double fs = f(s);
		d = c;
		c = b;
		fc = fb;

		if (fa * fs < 0) {
			b = s;
			fb = fs;
		}
		else {
			a = s;
			fa = fs;
		}

		if (fabs(fa) < fabs(fb)) {
			double swap = a;
			a = b;
			b = swap;

			swap = fa;
			fa = fb;
			fb = swap;
		}

		if (fb == 0) {
			return b;
		}

		if (fs == 0 || fabs(b - a) <= tol) {
			return s;
		}
	}

	r->error = NO_CONVERGENCE;
	return 0;
}

double f(double x) {
	double y = 3 * pow(x, 3) - 2 * pow(x, 2) + 1 * pow(x, 1) - 5;
	return y;
}

double f_derive(double x) {
	double y = 9 * pow(x, 2) - 4 * pow(x, 1) + 1;
	return y;
}

double g(double x) {
	return (x + 3) * (x - 1) * (x - 1);
}

// Derived by Kepler in 1609, one of first transcendental eqs
// Newton used his iterative method by hand to solve
//
double h(double x) {
	return x - sin(x) / 2 - 1;
}

int main(void) {
	result r;
	double root;

	//root = newton_raphson(f, f_derive, 2.0, 1.e-10, 50, &r);
	//printf("newton %d steps %f\n", r.iterations, root);
	//root = bisect(f, -2, 2, 1.e-10, 50, &r);
	//printf("bisect %d steps %f\n", r.iterations, root);
	//root = secant(f, 2.0, 2.5, 1.e-10, 50, &r);
	//printf("secant %d steps %f\n", r.iterations, root);
	//root = false_position(f, -2, 2, 1.e-10, 50, &r);
	//printf("false_position %d steps %f\n", r.iterations, root);
	//root = ridders(f, -2, 2, 1.e-10, 50, &r);
	//printf("ridders %d steps %f\n", r.iterations, root);
	//root = brent(f, -2, 2, 1.e-10, 50, &r);
	//printf("brent %d steps %f\n", r.iterations, root);

	//root = brent(g, 4, 5, 1.e-12, 50, &r);
	//printf("brent %d steps %f\n", r.iterations, root);

	root = bisect(h, 0, 2, 1e-10, 50, &r);
	printf("bisect - %d steps, %.10f\n", r.iterations, root);
	root = secant(h, 0, 2, 1e-10, 50, &r);
	printf("secant - %d steps, %.10f\n", r.iterations, root);
	root = false_position(h, 0, 2, 1e-10, 50, &r);
	printf("false_position - %d steps, %.10f\n", r.iterations, root);
	root = ridders(h, 0, 2, 1e-10, 50, &r);
	printf("ridders - %d steps, %.10f\n", r.iterations, root);
	root = brent(h, 0, 2, 1e-10, 50, &r);
	printf("brent - %d steps, %.10f\n", r.iterations, root);

	getchar();

	return 0;
}