#include "Algorithm.h"

using std::vector;

double Algorithm::normal_CDF(double x)
{
   return 0.5 * erfc(-x * M_SQRT1_2);
}

double Algorithm::brent(Function1d& f, double x1, double x2, double tol, double f_x1 /*= NAN*/, double f_x2 /*= NAN*/)
{
	double a = x1, b = x2, c = x2, d = 0.0, e = 0.0, min1, min2;
	double fa = isnan(f_x1) ? f.evaluate(a) : f_x1;
	double fb = isnan(f_x2) ? f.evaluate(b) : f_x2;
	double fc, p, q, r, s, tol1, xm;

	fc = fb;
	for (int iter = 1; iter <= BRENT_ITMAX; iter++) {
		if ((fb > 0.0 && fc > 0.0) || (fb < 0.0 && fc < 0.0)) {
			c = a;
			fc = fa;
			e = d = b - a;
		}
		if (fabs(fc) < fabs(fb)) {
			a = b;
			b = c;
			c = a;
			fa = fb;
			fb = fc;
			fc = fa;
		}
		tol1 = 2.0 * BRENT_RTOL * fabs(b) + 0.5 * tol;
		xm = 0.5 * (c - b);
		if (fabs(xm) <= tol1 || fb == 0.0) return b;
		if (fabs(e) >= tol1 && fabs(fa) > fabs(fb))
		{
			s = fb / fa;
			if (a == c) {
				p = 2.0 * xm * s;
				q = 1.0 - s;
			}
			else {
				q = fa / fc;
				r = fb / fc;
				p = s * (2.0 * xm * q * (q - r) - (b - a) * (r - 1.0));
				q = (q - 1.0) * (r - 1.0) * (s - 1.0);
			}
			if (p > 0.0) q = -q;
			p = fabs(p);
			min1 = 3.0 * xm * q - fabs(tol1 * q);
			min2 = fabs(e * q);
			if (2.0 * p < (min1 < min2 ? min1 : min2))
			{
				e = d;
				d = p / q;
			}
			else {
				d = xm;
				e = d;
			}
		}
		else {
			d = xm;
			e = d;
		}
		a = b;
		fa = fb;
		if (fabs(d) > tol1)
			b += d;
		else
			b += xm >= 0.0 ? fabs(tol1) : -fabs(tol1);
		fb = f.evaluate(b);
	}

	throw std::runtime_error("Maximum number of iterations exceeded in zbrent");
}