#include "MultirootCalculator.h"

int single_foc_int_terminal_nk(const gsl_vector * x, void *params, gsl_vector * f)
{
	//Extract X:
	EquationCalculator * eqCalc = (EquationCalculator *)params;
	const double x0 = gsl_vector_get(x, 0);

	//Set F(X):
	const double y0 = EquationCalculator::foc_int_terminal_nk({ eqCalc, x0 });
	gsl_vector_set(f, 0, y0);

	//Report Success:
	return GSL_SUCCESS;
}

double solve_single_foc_int_terminal_nk(DoubleSingle X)
{
	const gsl_multiroot_fsolver_type *T;
	gsl_multiroot_fsolver *s;

	int status;
	size_t i, iter = 0;

	const size_t n = 1;
	gsl_multiroot_function f = { &single_foc_int_terminal_nk, n, X.eqCalc };

	double x_init[1] = { X.x0 };

	gsl_vector *x = gsl_vector_alloc(n);

	gsl_vector_set(x, 0, x_init[0]);


	T = gsl_multiroot_fsolver_hybrids;
	s = gsl_multiroot_fsolver_alloc(T, 1);
	gsl_multiroot_fsolver_set(s, &f, x);


	do
	{
		iter++;
		status = gsl_multiroot_fsolver_iterate(s);
		
		if (status)
			break;

		status = gsl_multiroot_test_residual(s->f, 1e-16);
	} while (status == GSL_CONTINUE && iter < 20000);

	double xVal = gsl_vector_get(s->x, 0);

	gsl_multiroot_fsolver_free(s);
	gsl_vector_free(x);

	return xVal;
}

int single_foc_int(const gsl_vector * x, void *params, gsl_vector * f)
{
	//Get X:
	EquationCalculator * eqCalc = (EquationCalculator *)params;
	DoublePair X = { eqCalc, gsl_vector_get(x, 0) , gsl_vector_get(x, 1) };

	//Evaluate F(X):
	DoublePair F = EquationCalculator::foc_int(X);

	//Set F(X):
	gsl_vector_set(f, 0, F.x0);
	gsl_vector_set(f, 1, F.x1);

	//Report Success:
	return GSL_SUCCESS;
}

DoublePair solve_single_foc_int(DoublePair X)
{
	#pragma region Solver Setup

	const gsl_multiroot_fsolver_type *T;
	gsl_multiroot_fsolver *s;

	int status, resDeltaStatus, resResidualStatus;

	size_t i, iter = 0;
	const size_t n = 2;
	T = gsl_multiroot_fsolver_hybrid;
	s = gsl_multiroot_fsolver_alloc(T, n);

	gsl_multiroot_function f = { &single_foc_int, n, X.eqCalc };
	gsl_vector *x = gsl_vector_alloc(n);

	#pragma endregion

	//Set Solver Parameters:
	gsl_vector_set(x, 0, X.x0);
	gsl_vector_set(x, 1, X.x1);
	gsl_multiroot_fsolver_set(s, &f, x);

	//Solver Iterate:
	do
	{
		iter++;
		status = gsl_multiroot_fsolver_iterate(s);

		if (status) break;
			
		status = gsl_multiroot_test_residual(s->f, 1e-16);
	} 
	while  (status == GSL_CONTINUE &&
			iter < 20000);

	//Assign result to the return value:
	DoublePair xVal = { X.eqCalc, gsl_vector_get(s->x, 0), gsl_vector_get(s->x, 1) };

	#pragma region Solver Return

	gsl_multiroot_fsolver_free(s);
	gsl_vector_free(x);

	return xVal;

	#pragma endregion
}

int single_foc_double_op(const gsl_vector * x, void *params, gsl_vector * f)
{
	//Get X:
	void ** box = (void **)params;
	EquationCalculator * eqCalc = (EquationCalculator *)box[0];
	DoublePair X = { eqCalc, gsl_vector_get(x, 0) , gsl_vector_get(x, 1) };
	FocDoubleOp func = (FocDoubleOp)box[1];

	//Evaluate F(X):
	DoublePair F = func(X);

	//Set F(X):
	gsl_vector_set(f, 0, F.x0);
	gsl_vector_set(f, 1, F.x1);

	//Report Success:
	return GSL_SUCCESS;
}

DoublePair solve_single_foc_double_op(DoublePair X, FocDoubleOp func)
{
#pragma region Solver Setup

	const gsl_multiroot_fsolver_type *T;
	gsl_multiroot_fsolver *s;

	int status, resDeltaStatus, resResidualStatus;

	size_t i, iter = 0;
	const size_t n = 2;
	T = gsl_multiroot_fsolver_hybrid;
	s = gsl_multiroot_fsolver_alloc(T, n);

	void * box[2];
	box[0] = (void*)X.eqCalc;
	box[1] = (void*)func;

	gsl_multiroot_function f = { &single_foc_double_op, n, box };
	gsl_vector *x = gsl_vector_alloc(n);

#pragma endregion

	//Set Solver Parameters:
	gsl_vector_set(x, 0, X.x0);
	gsl_vector_set(x, 1, X.x1);
	gsl_multiroot_fsolver_set(s, &f, x);

	//Solver Iterate:
	do
	{
		iter++;
		status = gsl_multiroot_fsolver_iterate(s);

		if (status) break;
		status = gsl_multiroot_test_residual(s->f, 1e-16);
	} while (status == GSL_CONTINUE &&
		iter < 20000);

	//Assign result to the return value:
	DoublePair xVal = { X.eqCalc, gsl_vector_get(s->x, 0), gsl_vector_get(s->x, 1), status };

#pragma region Solver Return

	gsl_multiroot_fsolver_free(s);
	gsl_vector_free(x);

	return xVal;

#pragma endregion
}

int single_foc_trio_op(const gsl_vector * x, void *params, gsl_vector * f)
{
	//Get X:
	void ** box = (void **)params;
	EquationCalculator * eqCalc = (EquationCalculator *)box[0];
	DoubleTrio X = { eqCalc, gsl_vector_get(x, 0) , gsl_vector_get(x, 1), gsl_vector_get(x, 2) };
	FocTrioOp func = (FocTrioOp)box[1];

	//Evaluate F(X):
	DoubleTrio F = func(X);

	//Set F(X):
	gsl_vector_set(f, 0, F.x0);
	gsl_vector_set(f, 1, F.x1);
	gsl_vector_set(f, 2, F.x2);

	//Report Success:
	return GSL_SUCCESS;
}

DoubleTrio solve_single_foc_trio_op(DoubleTrio X, FocTrioOp func)
{

#pragma region Solver Setup

	const gsl_multiroot_fsolver_type *T;
	gsl_multiroot_fsolver *s;

	int status, resDeltaStatus, resResidualStatus;

	size_t i, iter = 0;
	const size_t n = 3;
	T = gsl_multiroot_fsolver_hybrid;
	s = gsl_multiroot_fsolver_alloc(T, n);


	void * box[2];
	box[0] = (void*)X.eqCalc;
	box[1] = (void*)func;

	gsl_multiroot_function f = { &single_foc_trio_op, n, box };
	gsl_vector *x = gsl_vector_alloc(n);

#pragma endregion

	//Set Solver Parameters:
	gsl_vector_set(x, 0, X.x0);
	gsl_vector_set(x, 1, X.x1);
	gsl_vector_set(x, 2, X.x2);
	gsl_multiroot_fsolver_set(s, &f, x);

	//Solver Iterate:
	do
	{
		iter++;
		status = gsl_multiroot_fsolver_iterate(s);

		if (status) break;
		status = gsl_multiroot_test_residual(s->f, 1e-16);
	} while (status == GSL_CONTINUE &&
		iter < 20000);

	//Assign result to the return value:
	DoubleTrio xVal = { X.eqCalc, gsl_vector_get(s->x, 0), gsl_vector_get(s->x, 1), gsl_vector_get(s->x, 2), status };

#pragma region Solver Return

	gsl_multiroot_fsolver_free(s);
	gsl_vector_free(x);

	return xVal;

#pragma endregion
}

int single_foc_single_op(const gsl_vector * x, void *params, gsl_vector * f)
{
	//Get X:
	void ** box = (void **)params;
	EquationCalculator * eqCalc = (EquationCalculator *)box[0];
	DoubleSingle X = { eqCalc, gsl_vector_get(x, 0)  };
	FocSingleOp func = (FocSingleOp)box[1];

	//Evaluate F(X):
	DoubleSingle F = func(X);

	//Set F(X):
	gsl_vector_set(f, 0, F.x0);

	//Report Success:
	return GSL_SUCCESS;
}

DoubleSingle solve_single_foc_single_op(DoubleSingle X, FocSingleOp func)
{

#pragma region Solver Setup

	const gsl_multiroot_fsolver_type *T;
	gsl_multiroot_fsolver *s;

	int status, resDeltaStatus, resResidualStatus;

	size_t i, iter = 0;
	const size_t n = 1;
	T = gsl_multiroot_fsolver_hybrid;
	s = gsl_multiroot_fsolver_alloc(T, n);

	void * box[2];
	box[0] = (void*)X.eqCalc;
	box[1] = (void*)func;

	gsl_multiroot_function f = { &single_foc_single_op, n, box };
	gsl_vector *x = gsl_vector_alloc(n);

#pragma endregion

	//Set Solver Parameters:
	gsl_vector_set(x, 0, X.x0);
	gsl_multiroot_fsolver_set(s, &f, x);

	//Solver Iterate:
	do
	{
		iter++;
		status = gsl_multiroot_fsolver_iterate(s);

		if (status) break;
		status = gsl_multiroot_test_residual(s->f, 1e-16);
	} while (status == GSL_CONTINUE &&
		iter < 20000);

	//Assign result to the return value:
	DoubleSingle xVal = { X.eqCalc, gsl_vector_get(s->x, 0), status };

#pragma region Solver Return

	gsl_multiroot_fsolver_free(s);
	gsl_vector_free(x);

	return xVal;

#pragma endregion
}

void print_state(size_t iter, gsl_multiroot_fsolver * s)
{
	mexPrintFormat(1000, "iter = %3u x = % .3f, f(x) = % .3e\n", (int)iter, gsl_vector_get(s->x, 0), gsl_vector_get(s->f, 0));
}