#include<limits>
#include<cmath>
#include<functional>
#include"vector.h"
#include"symmetric_matrix.h"
namespace math{

typedef double Float;
static const Float Inf = std::numeric_limits<Float>::infinity();
static const Float epsilon = std::numeric_limits<Float>::epsilon();
static const Float u_forward = std::sqrt(epsilon);
static const Float u_center = std::cbrt(epsilon) * std::cbrt(epsilon);

inline bool almost_equal(Float x, Float y, int ulp = 2)
{
	return std::abs(x - y) <= std::max(std::abs(x), std::abs(y))
							  * epsilon * ulp;
}

inline bool almost_zero(Float x, int ulp = 10)
{
	return std::abs(x) <= epsilon * ulp;
}


typedef vector<Float> vector_type;
typedef std::function<Float(const vector_type&)> vector_function;
typedef vector<vector_function> gradient_function;
typedef symmetric_matrix<Float, lower> hessian_matrix;
typedef symmetric_matrix<Float, lower> jacobian_matrix;

enum DiffMethod{FORWARD, CENTER};
//gradient of a vector function
vector_type gradient(vector_function f, const vector_type &x, DiffMethod method = FORWARD)
{
	vector_type dx(x.size());
	switch(method)
	{
		case FORWARD:
		{
			vector_type x1(x);
			Float fx = f(x);
			for(int i = 0; i < x.size(); ++i)
			{
				auto tmp = x1[i];
				x1[i] += u_forward;
				dx[i] = (f(x1) - fx) / u_forward;
				x1[i] = tmp;
			}
			return dx;
			break;
		}
		case CENTER:
		{
			vector_type x1(x);
			Float yl, yu;
			for(int i = 0; i < x.size(); ++i)
			{
				auto tmp = x1[i];
				x1[i] += u_center;
				yu = f(x1);
				x1[i] -= 2 * u_center;
				yl = f(x1);
				dx[i] = (yu - yl) / (2 * u_center);
				x1[i] = tmp;
			}
			return dx;
			break;
		}
		default:
		{
			std::cout << "Difference method must be 0(Forward) or 1(CENTER)" << std::endl;
		}
	}
}

hessian_matrix hessian(gradient_function df, const vector_type &x, DiffMethod method = FORWARD)
{
	hessian_matrix H(x.size());
	switch(method)
	{
		case FORWARD:
		{
			vector_type x1(x);
			for(int i = 0; i < x.size(); ++i)
			{
				Float dfxi = df[i](x);
				for(int j = 0; j <= i; ++j)
				{
					auto tmp = x1[j];
					x1[j] += u_forward;
					H(i, j) = (df[i](x1) - dfxi) / u_forward;
					x1[j] = tmp;
				}
			}
			return H;
			break;
		}
		case CENTER:
		{
			vector_type x1(x);
			Float yl, yu;
			for(int i = 0; i < x.size(); ++i)
				for(int j = 0; j <= i; ++j)
				{
					auto tmp = x1[j];
					x1[j] += u_center;
					yu = df[i](x1);
					x1[j] -= u_center;
					x1[j] -= u_center;
					yl = df[i](x1);
					H(i, j) = (yu - yl) / (2 * u_center);
					x1[j] = tmp;
				}
			return H;
			break;
		}
		default:
		{
			std::cout << "Difference method must be 0(Forward) or 1(CENTER)" << std::endl;
		}
	}
}

}