#include "mexOptimization.h"
#include "EquationCalculator.h"
#include "MultirootCalculator.h"
#include "OptimizationParams.h"

void ExecCommand(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]);
void foc_int_terminal_nk(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]);
void foc_int(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]);
void solve_loop_E1_0(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]);
void solve_loop_E1_1(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]);
void solve_loop_E1_0_nk(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]);
void solve_loop_E1_1_nk(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]);
void foc_calc_single(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]);

SolutionSet foc_Solve_Single_0(OptimizationParams * oParams, EquationCalculator * eqCalc, int j, int k, int l, int m, int amin, int Aprev, int Agrid2);
SolutionSet foc_Solve_Single_1(OptimizationParams * oParams, EquationCalculator * eqCalc, int j, int k, int l, int m, int amin, int Aprev, int Agrid2);
SolutionSet foc_int_solve_std(EquationCalculator * eqCalc, int kttp1, double L1_star, double T1_star, double amin);
SolutionSet foc_int_solve_A(EquationCalculator * eqCalc, int kttp1, double L1_star, double T1_star);
SolutionSet foc_int_solve_H2(EquationCalculator * eqCalc, int kttp1, double L1_star, double amin);
SolutionSet foc_int_solve_H1(EquationCalculator * eqCalc, int kttp1, double L2_star, double amin);
SolutionSet foc_int_solve_A_H2(EquationCalculator * eqCalc, int kttp1, double L1_star, double amin);
SolutionSet foc_int_solve_A_H1(EquationCalculator * eqCalc, int kttp1, double L2_star, double amin);
SolutionSet foc_int_solve_H1_H2(EquationCalculator * eqCalc, int kttp1, double L1_star, double L2_star, double A_star, double Agrid2, double amin);
SolutionSet foc_int_solve_A_H1_H2(EquationCalculator * eqCalc);
SolutionSet foc_Solve_Single_0_nk(OptimizationParams * oParams, EquationCalculator * eqCalc, int j, int k, int l, int m, int amin, int Aprev, int Agrid2);
SolutionSet foc_Solve_Single_1_nk(OptimizationParams * oParams, EquationCalculator * eqCalc, int j, int k, int l, int m, int amin, int Aprev, int Agrid2);
SolutionSet foc_int_solve_std_nk(EquationCalculator * eqCalc, int kttp1, double L1_star, double T1_star, double amin);
SolutionSet foc_int_solve_A_nk(EquationCalculator * eqCalc, int kttp1, double L1_star, double T1_star);
SolutionSet foc_int_solve_H2_nk(EquationCalculator * eqCalc, int kttp1, double L1_star, double amin);
SolutionSet foc_int_solve_H1_nk(EquationCalculator * eqCalc, int kttp1, double L2_star, double amin);
SolutionSet foc_int_solve_A_H2_nk(EquationCalculator * eqCalc, int kttp1, double L1_star, double amin);
SolutionSet foc_int_solve_A_H1_nk(EquationCalculator * eqCalc, int kttp1, double L2_star, double amin);
SolutionSet foc_int_solve_H1_H2_nk(EquationCalculator * eqCalc, int kttp1, double L1_star, double L2_star, double A_star, double Agrid2, double amin);
SolutionSet foc_int_solve_A_H1_H2_nk(EquationCalculator * eqCalc);

void mexPrintSolutionSet(SolutionSet * Set, int j, int k, int l, int m);

bool validateParamCount(int input, int givenInputs, int output, int givenOutput);

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	ExecCommand(nlhs, plhs, nrhs, prhs);
}

void ExecCommand(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	IndexMap map = { map.command = 0 };
	OptimizationParams * oParams = new OptimizationParams();
	oParams->SetParamsWithMap(nrhs, prhs, map);

	switch (oParams->GetCommand())
	{
	case mexCommand::enum_foc_int:
		foc_int(oParams, nlhs, plhs, nrhs, prhs);
		break;

	case mexCommand::enum_solve_loop_E1_0:
		solve_loop_E1_0(oParams, nlhs, plhs, nrhs, prhs);
		break;

	case mexCommand::enum_solve_loop_E1_1:
		solve_loop_E1_1(oParams, nlhs, plhs, nrhs, prhs);
		break;

	case mexCommand::enum_solve_loop_E1_0_nk:
		solve_loop_E1_0_nk(oParams, nlhs, plhs, nrhs, prhs);
		break;

	case mexCommand::enum_solve_loop_E1_1_nk:
		solve_loop_E1_1_nk(oParams, nlhs, plhs, nrhs, prhs);
		break;

	case mexCommand::enum_foc_int_terminal_nk:
		foc_int_terminal_nk(oParams, nlhs, plhs, nrhs, prhs);
		break;

	case mexCommand::enum_foc_calc_single:
		foc_calc_single(oParams, nlhs, plhs, nrhs, prhs);
		break;

	default:
		mexErrMsgTxt("Requested command not implemented.\n");
	}

	delete(oParams);
}

void foc_int_terminal_nk(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	if (!validateParamCount(11, nrhs, 2, nlhs))
		return;

	int wpoints = oParams->Get_wpoints();
	int upoints = oParams->Get_upoints();
	int const dim = 4;
	mwSize dims[dim] = { wpoints ,wpoints ,upoints, upoints };

	plhs[0] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); 
	plhs[1] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); 

	double  * pL1temp = mxGetPr(plhs[0]); 
	double  * pFVAL = mxGetPr(plhs[1]); 

	EquationCalculator * eqCalc = new EquationCalculator();

	int * offsets = mexGetOffsetArray(dim, dims);

	for (int j = 0; j < wpoints; j++)
	{
		for (int k = 0; k < wpoints; k++)
		{
			for (int l = 0; l < upoints; l++)
			{
				for (int m = 0; m < upoints; m++)
				{
					eqCalc->SetLoopState(oParams, j, k, l, m);

					double fval = 1;
					int miter = 1;
					double fval_bnd = pow(10, -8);
					double miter_bnd = 4;

					DoubleSingle LT = { eqCalc, 0 };

					while (fval > fval_bnd && miter < miter_bnd)
					{
						DoubleSingle LT1_0 = { eqCalc, 1000 };
						LT.x0 = solve_single_foc_int_terminal_nk(LT1_0);
						fval = EquationCalculator::foc_int_terminal_nk(LT);
						miter = miter + 1;
					}

					int index = getFlatIndex(dim, offsets, j, k, l, m);

					pL1temp[index] = LT.x0;
					pFVAL[index] = fval;
				}
			}
		}
	}
	delete(eqCalc);
	delete(offsets);
}

void foc_int(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	if (!validateParamCount(15, nrhs, 7, nlhs))
		return;

	int wpoints = oParams->Get_wpoints();
	int upoints = oParams->Get_upoints();
	int const dim = 4;
	mwSize dims[dim] = { wpoints ,wpoints ,upoints, upoints };

	plhs[0] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); 
	plhs[1] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); 
	plhs[2] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); 
	plhs[3] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); 
	plhs[4] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); 
	plhs[5] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); 
	plhs[6] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); 
	
	double  * pL1temp = mxGetPr(plhs[0]); 
	double  * pT1temp = mxGetPr(plhs[1]); 
	double  * pFVAL = mxGetPr(plhs[2]);  
	double  * pL2temp = mxGetPr(plhs[3]); 
	double  * pT2temp = mxGetPr(plhs[4]); 
	double  * pAtemp = mxGetPr(plhs[5]); 
	double  * pVtemp = mxGetPr(plhs[6]); 

	EquationCalculator * eqCalc = new EquationCalculator();

	int * offsets = mexGetOffsetArray(dim, dims);
	DoublePair fval, LT, LTres;
	double L_bar = oParams->GetStructParamInt("L_bar");

	for (int j = 0; j < wpoints; j++)
	{
		for (int k = 0; k < wpoints; k++)
		{
			for (int l = 0; l < upoints; l++)
			{
				for (int m = 0; m < upoints; m++)
				{
					eqCalc->SetLoopState(oParams, j, k, l, m);
					eqCalc->SetNormSwitch(0);

					double L1_star = oParams->Get_L1_star_f_val(j, k, l, m);
					double T1_star = oParams->Get_T1_star_f_val(j, k, l, m);

					LT.eqCalc = eqCalc;
					LT.x0 = L1_star;
					LT.x1 = T1_star + 0.01;
					LTres = solve_single_foc_int(LT);

					fval = EquationCalculator::foc_int(LTres);
					double sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;


					if (sqRes > pow(10, -5))
					{
						if (abs(fval.x0) > abs(fval.x1))
							eqCalc->SetNormSwitch(1);
						else
							eqCalc->SetNormSwitch(2);

						LTres = solve_single_foc_int(LT);
						fval = EquationCalculator::foc_int(LTres);
						sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;

						if (sqRes > pow(10, -5))
						{
							LT.x0 = L1_star - 0.1;
							LT.x1 = T1_star + 0.1;
							LTres = solve_single_foc_int(LT);
							fval = EquationCalculator::foc_int(LTres);
							sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;
						}
					}

					double L1sol = LTres.x0;
					double T1sol = LTres.x1;
					double L2sol = eqCalc->calc_L2(L1sol);
					double T2sol = eqCalc->calc_T2(T1sol);
					double H1sol = L_bar - L1sol - T1sol;
					double H2sol = L_bar - L2sol - T2sol;
					DoublePair AC = eqCalc->calc_A(L1sol, T1sol, L2sol, T2sol);
					double Asol = AC.x0;
					double Csol = AC.x1;
					double Vsol = eqCalc->calc_V(L1sol, T1sol, L2sol, T2sol, Csol, Asol);

					int index = getFlatIndex(dim, offsets, j, k, l, m);

					pL1temp[index] = L1sol;
					pT1temp[index] = T1sol;
					pFVAL[index] = sqRes;
					pL2temp[index] = L2sol;
					pT2temp[index] = T2sol;
					pAtemp[index] = Asol;
					pVtemp[index] = Vsol;
				}
			}
		}
	}
	delete(eqCalc);
	delete(offsets);
}

/*
Mex Accepted Params:

Return Values: 
 - L1_star_temp
 - T1_star_temp
 - L2_star_temp
 - T2_star_temp
 - H1_star_temp
 - H2_star_temp
 - Y_star_temp
 - atY_star_temp
 - A_star_temp
 - C_star_temp
 - V_star_temp
 - FVAL_temp
 - CORNER_temp
 - U_star_temp
 - LAM1_star_temp
 - LAM2_star_temp
*/
void solve_loop_E1_0(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	if (nlhs == 16)
		mexPrintf("Warning, you are probably missing a Status output \n");

	if (!validateParamCount(18, nrhs, 18, nlhs))
		return;

	//Declarations:
	double L1_star, L2_star, T1_star, A_star;

	//Set up output matrix parameters:
	int wpoints = oParams->Get_wpoints();
	int upoints = oParams->Get_upoints();
	int const dim = 4;
	mwSize dims[dim] = { wpoints ,wpoints ,upoints, upoints };
	
	//Create output Matrices:
#pragma region Result Pointer Setup
	plhs[0] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - L1_star_temp
	double  * pL1_star_temp = mxGetPr(plhs[0]); //Output Matrix Pointer.

	plhs[1] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - T1_star_temp
	double  * pT1_star_temp = mxGetPr(plhs[1]); //Output Matrix Pointer.

	plhs[2] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - L2_star_temp
	double  * pL2_star_temp = mxGetPr(plhs[2]); //Output Matrix Pointer.

	plhs[3] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - T2_star_temp
	double  * pT2_star_temp = mxGetPr(plhs[3]); //Output Matrix Pointer.

	plhs[4] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - H1_star_temp
	double  * pH1_star_temp = mxGetPr(plhs[4]); //Output Matrix Pointer.

	plhs[5] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - H2_star_temp
	double  * pH2_star_temp = mxGetPr(plhs[5]); //Output Matrix Pointer.

	plhs[6] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - Y_star_temp
	double  * pY_star_temp = mxGetPr(plhs[6]); //Output Matrix Pointer.

	plhs[7] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - atY_star_temp
	double  * pAtY_star_temp = mxGetPr(plhs[7]); //Output Matrix Pointer.

	plhs[8] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - A_star_temp
	double  * pA_star_temp = mxGetPr(plhs[8]); //Output Matrix Pointer.

	plhs[9] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - C_star_temp
	double  * pC_star_temp = mxGetPr(plhs[9]); //Output Matrix Pointer.

	plhs[10] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - V_star_temp
	double  * pV_star_temp = mxGetPr(plhs[10]); //Output Matrix Pointer.

	plhs[11] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - FVAL_temp
	double  * pFVAL_temp = mxGetPr(plhs[11]); //Output Matrix Pointer.

	plhs[12] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - CORNER_temp
	double  * pCORNER_temp = mxGetPr(plhs[12]); //Output Matrix Pointer.

	plhs[13] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - U_star_temp
	double  * pU_star_temp = mxGetPr(plhs[13]); //Output Matrix Pointer.

	plhs[14] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - LAM1_star_temp
	double  * pLAM1_star_temp = mxGetPr(plhs[14]); //Output Matrix Pointer.

	plhs[15] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - LAM2_star_temp
	double  * pLAM2_star_temp = mxGetPr(plhs[15]); //Output Matrix Pointer.

	plhs[16] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - Algorithm Status
	double  * pStatus = mxGetPr(plhs[16]); //Output Matrix Pointer.

	plhs[17] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - EUFunc Results
	double  * pEU = mxGetPr(plhs[17]); //Output Matrix Pointer.
#pragma endregion

	EquationCalculator * eqCalc = new EquationCalculator();

	int * offsets = mexGetOffsetArray(dim, dims);
	DoublePair fval, LT, LTres;
	SolutionSet solutions[8];
	SolutionSet res;

	SolutionSet UNUSED;
	UNUSED.V = -DBL_MAX;

	double L_bar = oParams->GetStructParamInt("L_bar");
	double amin = oParams->GetStructParamDouble("amin");
	double Aprev = oParams->Get_A_grid_val(oParams->Get_i());
	double Agrid2 = oParams->Get_A_grid_val(1); 

	for (int j = 0; j < wpoints; j++)
	{
		for (int k = 0; k < wpoints; k++)
		{
			for (int l = 0; l < upoints; l++)
			{
				for (int m = 0; m < upoints; m++)
				{
					res = foc_Solve_Single_0(oParams, eqCalc, j, k, l, m, amin, Aprev, Agrid2);
					
					int index = getFlatIndex(dim, offsets, j, k, l, m);

					pL1_star_temp[index] = res.L1;
					pT1_star_temp[index] = res.T1;
					pL2_star_temp[index] = res.L2;
					pT2_star_temp[index] = res.T2;
					pH1_star_temp[index] = res.H1;
					pH2_star_temp[index] = res.H2;
					pY_star_temp[index] = res.Y;  
					pAtY_star_temp[index] = res.atY;
					pA_star_temp[index] = res.A;    
					pC_star_temp[index] = res.C;    
					pV_star_temp[index] = res.V;    
					pFVAL_temp[index] = res.F;      
					pCORNER_temp[index] = res.CorIndex;
					pU_star_temp[index] = res.U;       
					pLAM1_star_temp[index] = res.LAM1; 
					pLAM2_star_temp[index] = res.LAM2; 
					pStatus[index] = res.status;
					pEU[index] = res.EU;
				}
			}
		}
	}
	delete(eqCalc);
	delete(offsets);
}

void solve_loop_E1_1(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	if (nlhs == 16)
		mexPrintf("Warning, you are probably missing a Status output \n");

	if (!validateParamCount(18, nrhs, 18, nlhs))
		return;

	double L1_star, L2_star, T1_star, A_star;

	int wpoints = oParams->Get_wpoints();
	int upoints = oParams->Get_upoints();
	int const dim = 4;
	mwSize dims[dim] = { wpoints ,wpoints ,upoints, upoints };

#pragma region Result Pointer Setup
	plhs[0] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - L1_star_temp
	double  * pL1_star_temp = mxGetPr(plhs[0]); //Output Matrix Pointer.

	plhs[1] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - T1_star_temp
	double  * pT1_star_temp = mxGetPr(plhs[1]); //Output Matrix Pointer.

	plhs[2] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - L2_star_temp
	double  * pL2_star_temp = mxGetPr(plhs[2]); //Output Matrix Pointer.

	plhs[3] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - T2_star_temp
	double  * pT2_star_temp = mxGetPr(plhs[3]); //Output Matrix Pointer.

	plhs[4] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - H1_star_temp
	double  * pH1_star_temp = mxGetPr(plhs[4]); //Output Matrix Pointer.

	plhs[5] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - H2_star_temp
	double  * pH2_star_temp = mxGetPr(plhs[5]); //Output Matrix Pointer.

	plhs[6] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - Y_star_temp
	double  * pY_star_temp = mxGetPr(plhs[6]); //Output Matrix Pointer.

	plhs[7] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - atY_star_temp
	double  * pAtY_star_temp = mxGetPr(plhs[7]); //Output Matrix Pointer.

	plhs[8] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - A_star_temp
	double  * pA_star_temp = mxGetPr(plhs[8]); //Output Matrix Pointer.

	plhs[9] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - C_star_temp
	double  * pC_star_temp = mxGetPr(plhs[9]); //Output Matrix Pointer.

	plhs[10] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - V_star_temp
	double  * pV_star_temp = mxGetPr(plhs[10]); //Output Matrix Pointer.

	plhs[11] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - FVAL_temp
	double  * pFVAL_temp = mxGetPr(plhs[11]); //Output Matrix Pointer.

	plhs[12] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - CORNER_temp
	double  * pCORNER_temp = mxGetPr(plhs[12]); //Output Matrix Pointer.

	plhs[13] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - U_star_temp
	double  * pU_star_temp = mxGetPr(plhs[13]); //Output Matrix Pointer.

	plhs[14] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - LAM1_star_temp
	double  * pLAM1_star_temp = mxGetPr(plhs[14]); //Output Matrix Pointer.

	plhs[15] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - LAM2_star_temp
	double  * pLAM2_star_temp = mxGetPr(plhs[15]); //Output Matrix Pointer.

	plhs[16] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - Algorithm Status
	double  * pStatus = mxGetPr(plhs[16]); //Output Matrix Pointer.

	plhs[17] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - EUFunc Results
	double  * pEU = mxGetPr(plhs[17]); //Output Matrix Pointer.
#pragma endregion

	EquationCalculator * eqCalc = new EquationCalculator();

	int * offsets = mexGetOffsetArray(dim, dims);

	DoublePair fval, LT, LTres;
	SolutionSet solutions[8];
	SolutionSet res;

	SolutionSet UNUSED;
	UNUSED.V = -DBL_MAX;

	double L_bar = oParams->GetStructParamInt("L_bar");
	double amin = oParams->GetStructParamDouble("amin");
	double Aprev = oParams->Get_A_grid_val(oParams->Get_i());
	double Agrid2 = oParams->Get_A_grid_val(1); 

	for (int j = 0; j < wpoints; j++)
	{
		for (int k = 0; k < wpoints; k++)
		{
			for (int l = 0; l < upoints; l++)
			{
				for (int m = 0; m < upoints; m++)
				{
					res = foc_Solve_Single_1(oParams, eqCalc, j, k, l, m, amin, Aprev, Agrid2);
					
					int index = getFlatIndex(dim, offsets, j, k, l, m);

					pL1_star_temp[index] = res.L1;
					pT1_star_temp[index] = res.T1;
					pL2_star_temp[index] = res.L2;
					pT2_star_temp[index] = res.T2;
					pH1_star_temp[index] = res.H1;
					pH2_star_temp[index] = res.H2;
					pY_star_temp[index] = res.Y;  
					pAtY_star_temp[index] = res.atY;
					pA_star_temp[index] = res.A;    
					pC_star_temp[index] = res.C;	
					pV_star_temp[index] = res.V;	
					pFVAL_temp[index] = res.F;		
					pCORNER_temp[index] = res.CorIndex;
					pU_star_temp[index] = res.U;		
					pLAM1_star_temp[index] = res.LAM1; 
					pLAM2_star_temp[index] = res.LAM2; 
					pStatus[index] = res.status;
					pEU[index] = res.EU;
				}
			}
		}
	}
	delete(eqCalc);
	delete(offsets);
}

void solve_loop_E1_0_nk(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	if (nlhs == 16)
		mexPrintf("Warning, you are probably missing a Status output \n");

	if (!validateParamCount(18, nrhs, 18, nlhs))
		return;

	double L1_star, L2_star, T1_star, A_star;

	int wpoints = oParams->Get_wpoints();
	int upoints = oParams->Get_upoints();
	int const dim = 4;
	mwSize dims[dim] = { wpoints ,wpoints ,upoints, upoints };

#pragma region Result Pointer Setup
	plhs[0] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - L1_star_temp
	double  * pL1_star_temp = mxGetPr(plhs[0]); //Output Matrix Pointer.

	plhs[1] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - T1_star_temp
	double  * pT1_star_temp = mxGetPr(plhs[1]); //Output Matrix Pointer.

	plhs[2] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - L2_star_temp
	double  * pL2_star_temp = mxGetPr(plhs[2]); //Output Matrix Pointer.

	plhs[3] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - T2_star_temp
	double  * pT2_star_temp = mxGetPr(plhs[3]); //Output Matrix Pointer.

	plhs[4] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - H1_star_temp
	double  * pH1_star_temp = mxGetPr(plhs[4]); //Output Matrix Pointer.

	plhs[5] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - H2_star_temp
	double  * pH2_star_temp = mxGetPr(plhs[5]); //Output Matrix Pointer.

	plhs[6] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - Y_star_temp
	double  * pY_star_temp = mxGetPr(plhs[6]); //Output Matrix Pointer.

	plhs[7] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - atY_star_temp
	double  * pAtY_star_temp = mxGetPr(plhs[7]); //Output Matrix Pointer.

	plhs[8] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - A_star_temp
	double  * pA_star_temp = mxGetPr(plhs[8]); //Output Matrix Pointer.

	plhs[9] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - C_star_temp
	double  * pC_star_temp = mxGetPr(plhs[9]); //Output Matrix Pointer.

	plhs[10] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - V_star_temp
	double  * pV_star_temp = mxGetPr(plhs[10]); //Output Matrix Pointer.

	plhs[11] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - FVAL_temp
	double  * pFVAL_temp = mxGetPr(plhs[11]); //Output Matrix Pointer.

	plhs[12] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - CORNER_temp
	double  * pCORNER_temp = mxGetPr(plhs[12]); //Output Matrix Pointer.

	plhs[13] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - U_star_temp
	double  * pU_star_temp = mxGetPr(plhs[13]); //Output Matrix Pointer.

	plhs[14] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - LAM1_star_temp
	double  * pLAM1_star_temp = mxGetPr(plhs[14]); //Output Matrix Pointer.

	plhs[15] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - LAM2_star_temp
	double  * pLAM2_star_temp = mxGetPr(plhs[15]); //Output Matrix Pointer.

	plhs[16] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - Algorithm Status
	double  * pStatus = mxGetPr(plhs[16]); //Output Matrix Pointer.

	plhs[17] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - EUFunc Results
	double  * pEU = mxGetPr(plhs[17]); //Output Matrix Pointer.
#pragma endregion

	EquationCalculator * eqCalc = new EquationCalculator();

	int * offsets = mexGetOffsetArray(dim, dims);

	DoublePair fval, LT, LTres;
	SolutionSet solutions[8];
	SolutionSet res;

	SolutionSet UNUSED;
	UNUSED.V = -DBL_MAX;

	double L_bar = oParams->GetStructParamInt("L_bar");
	double amin = oParams->GetStructParamDouble("amin");
	double Aprev = oParams->Get_A_grid_val(oParams->Get_i());
	double Agrid2 = oParams->Get_A_grid_val(1); 

	for (int j = 0; j < wpoints; j++)
	{
		for (int k = 0; k < wpoints; k++)
		{
			for (int l = 0; l < upoints; l++)
			{
				for (int m = 0; m < upoints; m++)
				{
					res = foc_Solve_Single_0_nk(oParams, eqCalc, j, k, l, m, amin, Aprev, Agrid2);

					int index = getFlatIndex(dim, offsets, j, k, l, m);

					pL1_star_temp[index] = res.L1;  
					pT1_star_temp[index] = res.T1;  
					pL2_star_temp[index] = res.L2;  
					pT2_star_temp[index] = res.T2;  
					pH1_star_temp[index] = res.H1;  
					pH2_star_temp[index] = res.H2;  
					pY_star_temp[index] = res.Y;  
					pAtY_star_temp[index] = res.atY;  
					pA_star_temp[index] = res.A;  
					pC_star_temp[index] = res.C;  
					pV_star_temp[index] = res.V;  
					pFVAL_temp[index] = res.F;  
					pCORNER_temp[index] = res.CorIndex;  
					pU_star_temp[index] = res.U;  
					pLAM1_star_temp[index] = res.LAM1;  
					pLAM2_star_temp[index] = res.LAM2; 
					pStatus[index] = res.status;
					pEU[index] = res.EU;
				}
			}
		}
	}

	delete(eqCalc);
	delete(offsets);
}

void solve_loop_E1_1_nk(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	if (nlhs == 16)
		mexPrintf("Warning, you are probably missing a Status output \n");

	if (!validateParamCount(18, nrhs, 18, nlhs))
		return;
	double L1_star, L2_star, T1_star, A_star;

	int wpoints = oParams->Get_wpoints();
	int upoints = oParams->Get_upoints();
	int const dim = 4;
	mwSize dims[dim] = { wpoints ,wpoints ,upoints, upoints };

	//Create output Matrices:
#pragma region Result Pointer Setup
	plhs[0] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - L1_star_temp
	double  * pL1_star_temp = mxGetPr(plhs[0]); //Output Matrix Pointer.

	plhs[1] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - T1_star_temp
	double  * pT1_star_temp = mxGetPr(plhs[1]); //Output Matrix Pointer.

	plhs[2] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - L2_star_temp
	double  * pL2_star_temp = mxGetPr(plhs[2]); //Output Matrix Pointer.

	plhs[3] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - T2_star_temp
	double  * pT2_star_temp = mxGetPr(plhs[3]); //Output Matrix Pointer.

	plhs[4] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - H1_star_temp
	double  * pH1_star_temp = mxGetPr(plhs[4]); //Output Matrix Pointer.

	plhs[5] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - H2_star_temp
	double  * pH2_star_temp = mxGetPr(plhs[5]); //Output Matrix Pointer.

	plhs[6] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - Y_star_temp
	double  * pY_star_temp = mxGetPr(plhs[6]); //Output Matrix Pointer.

	plhs[7] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - atY_star_temp
	double  * pAtY_star_temp = mxGetPr(plhs[7]); //Output Matrix Pointer.

	plhs[8] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - A_star_temp
	double  * pA_star_temp = mxGetPr(plhs[8]); //Output Matrix Pointer.

	plhs[9] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - C_star_temp
	double  * pC_star_temp = mxGetPr(plhs[9]); //Output Matrix Pointer.

	plhs[10] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - V_star_temp
	double  * pV_star_temp = mxGetPr(plhs[10]); //Output Matrix Pointer.

	plhs[11] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - FVAL_temp
	double  * pFVAL_temp = mxGetPr(plhs[11]); //Output Matrix Pointer.

	plhs[12] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - CORNER_temp
	double  * pCORNER_temp = mxGetPr(plhs[12]); //Output Matrix Pointer.

	plhs[13] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - U_star_temp
	double  * pU_star_temp = mxGetPr(plhs[13]); //Output Matrix Pointer.

	plhs[14] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - LAM1_star_temp
	double  * pLAM1_star_temp = mxGetPr(plhs[14]); //Output Matrix Pointer.

	plhs[15] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - LAM2_star_temp
	double  * pLAM2_star_temp = mxGetPr(plhs[15]); //Output Matrix Pointer.

	plhs[16] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - Algorithm Status
	double  * pStatus = mxGetPr(plhs[16]); //Output Matrix Pointer.

	plhs[17] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - EUFunc Results
	double  * pEU = mxGetPr(plhs[17]); //Output Matrix Pointer.
#pragma endregion

	EquationCalculator * eqCalc = new EquationCalculator();

	int * offsets = mexGetOffsetArray(dim, dims);

	DoublePair fval, LT, LTres;
	SolutionSet solutions[8];
	SolutionSet res;

	SolutionSet UNUSED;
	UNUSED.V = -DBL_MAX;

	double L_bar = oParams->GetStructParamInt("L_bar");
	double amin = oParams->GetStructParamDouble("amin");
	double Aprev = oParams->Get_A_grid_val(oParams->Get_i());
	double Agrid2 = oParams->Get_A_grid_val(1); 

	for (int j = 0; j < wpoints; j++)
	{
		for (int k = 0; k < wpoints; k++)
		{
			for (int l = 0; l < upoints; l++)
			{
				for (int m = 0; m < upoints; m++)
				{
					res = foc_Solve_Single_1_nk(oParams, eqCalc, j, k, l, m, amin, Aprev, Agrid2);

					int index = getFlatIndex(dim, offsets, j, k, l, m);

					pL1_star_temp[index] = res.L1;  
					pT1_star_temp[index] = res.T1;  
					pL2_star_temp[index] = res.L2;  
					pT2_star_temp[index] = res.T2;  
					pH1_star_temp[index] = res.H1;  
					pH2_star_temp[index] = res.H2;  
					pY_star_temp[index] = res.Y;  
					pAtY_star_temp[index] = res.atY;  
					pA_star_temp[index] = res.A;  
					pC_star_temp[index] = res.C;  
					pV_star_temp[index] = res.V;  
					pFVAL_temp[index] = res.F;  
					pCORNER_temp[index] = res.CorIndex;  
					pU_star_temp[index] = res.U;  
					pLAM1_star_temp[index] = res.LAM1;  
					pLAM2_star_temp[index] = res.LAM2; 
					pStatus[index] = res.status;
					pEU[index] = res.EU;
				}
			}
		}
	}

	delete(eqCalc);
	delete(offsets);
}

void foc_calc_single(OptimizationParams * oParams, int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
	if (!validateParamCount(23, nrhs, 18, nlhs))
		return;

#pragma region Result Pointer Setup
	int const dim = 1;
	mwSize dims[1] = { 1 };

	plhs[0] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - L1_star_temp
	double  * pL1_star_temp = mxGetPr(plhs[0]); //Output Matrix Pointer.

	plhs[1] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - T1_star_temp
	double  * pT1_star_temp = mxGetPr(plhs[1]); //Output Matrix Pointer.

	plhs[2] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - L2_star_temp
	double  * pL2_star_temp = mxGetPr(plhs[2]); //Output Matrix Pointer.

	plhs[3] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - T2_star_temp
	double  * pT2_star_temp = mxGetPr(plhs[3]); //Output Matrix Pointer.

	plhs[4] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - H1_star_temp
	double  * pH1_star_temp = mxGetPr(plhs[4]); //Output Matrix Pointer.

	plhs[5] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - H2_star_temp
	double  * pH2_star_temp = mxGetPr(plhs[5]); //Output Matrix Pointer.

	plhs[6] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - Y_star_temp
	double  * pY_star_temp = mxGetPr(plhs[6]); //Output Matrix Pointer.

	plhs[7] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - atY_star_temp
	double  * pAtY_star_temp = mxGetPr(plhs[7]); //Output Matrix Pointer.

	plhs[8] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - A_star_temp
	double  * pA_star_temp = mxGetPr(plhs[8]); //Output Matrix Pointer.

	plhs[9] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - C_star_temp
	double  * pC_star_temp = mxGetPr(plhs[9]); //Output Matrix Pointer.

	plhs[10] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - V_star_temp
	double  * pV_star_temp = mxGetPr(plhs[10]); //Output Matrix Pointer.

	plhs[11] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - FVAL_temp
	double  * pFVAL_temp = mxGetPr(plhs[11]); //Output Matrix Pointer.

	plhs[12] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - CORNER_temp
	double  * pCORNER_temp = mxGetPr(plhs[12]); //Output Matrix Pointer.

	plhs[13] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - U_star_temp
	double  * pU_star_temp = mxGetPr(plhs[13]); //Output Matrix Pointer.

	plhs[14] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - LAM1_star_temp
	double  * pLAM1_star_temp = mxGetPr(plhs[14]); //Output Matrix Pointer.

	plhs[15] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - LAM2_star_temp
	double  * pLAM2_star_temp = mxGetPr(plhs[15]); //Output Matrix Pointer.

	plhs[16] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - Algorithm Status
	double  * pStatus = mxGetPr(plhs[16]); //Output Matrix Pointer.

	plhs[17] = mxCreateNumericArray(dim, dims, mxDOUBLE_CLASS, mxREAL); // - EUFunc Results
	double  * pEU = mxGetPr(plhs[17]); //Output Matrix Pointer.
#pragma endregion

	EquationCalculator * eqCalc = new EquationCalculator();

	DoublePair fval, LT, LTres;
	SolutionSet solutions[8];
	SolutionSet res;

	SolutionSet UNUSED;
	UNUSED.V = -DBL_MAX;

	double L_bar = oParams->GetStructParamInt("L_bar");
	double amin = oParams->GetStructParamDouble("amin");
	double Aprev = oParams->Get_A_grid_val(oParams->Get_i());
	double Agrid2 = oParams->Get_A_grid_val(1); 

	int j = oParams->Get_j();
	int k = oParams->Get_k();
	int l = oParams->Get_l();
	int m = oParams->Get_m();

	res = foc_Solve_Single_1(oParams, eqCalc, j, k, l, m, amin, Aprev, Agrid2);
	int index = 0;

	pL1_star_temp[index] = res.L1;  
	pT1_star_temp[index] = res.T1;  
	pL2_star_temp[index] = res.L2;  
	pT2_star_temp[index] = res.T2;  
	pH1_star_temp[index] = res.H1;  
	pH2_star_temp[index] = res.H2;  
	pY_star_temp[index] = res.Y;  
	pAtY_star_temp[index] = res.atY;  
	pA_star_temp[index] = res.A;  
	pC_star_temp[index] = res.C;  
	pV_star_temp[index] = res.V;  
	pFVAL_temp[index] = res.F;  
	pCORNER_temp[index] = res.CorIndex;  
	pU_star_temp[index] = res.U;  
	pLAM1_star_temp[index] = res.LAM1; 
	pLAM2_star_temp[index] = res.LAM2; 
	pStatus[index] = res.status;
	pEU[index] = res.EU;
	delete(eqCalc);
}

SolutionSet foc_Solve_Single_0(OptimizationParams * oParams, EquationCalculator * eqCalc, int j, int k, int l, int m, int amin, int Aprev, int Agrid2)
{	
	int printSols = oParams->Get_print_level() != printLevel::enum_printLevel_unknown;

	if (printSols)
		mexPrintf("~~~~~~~~ CURRENTLY SOLVING (%i,%i,%i,%i): ~~~~~~~~\n", j, k, l, m);

	DoublePair fval, LT, LTres;
	SolutionSet solutions[8];
	SolutionSet res;

	SolutionSet UNUSED;
	UNUSED.V = -DBL_MAX;
	UNUSED.status = -3;

	eqCalc->SetLoopState(oParams, j, k, l, m);
	double L1_star = oParams->Get_L1_star_f_val(j, k, l, m);
	double L2_star = oParams->Get_L2_star_f_val(j, k, l, m);
	double T1_star = oParams->Get_T1_star_f_val(j, k, l, m);
	double A_star = oParams->Get_A_star_f_val(j, k, l, m);
	int kttp1 = eqCalc->GetKttp();

	res.V = -DBL_MAX;

	solutions[0] = UNUSED;

	solutions[1] = UNUSED;

	solutions[2] = foc_int_solve_H2(eqCalc, kttp1, L1_star, amin);
	if (printSols && (solutions[2].status != UNUSED.status))
		mexPrintSolutionSet(&solutions[2], j, k, l, m);

	solutions[3] = UNUSED;

	solutions[4] = (solutions[2].A < amin) ? foc_int_solve_A_H2(eqCalc, kttp1, L1_star, amin) : UNUSED;
	if (printSols && (solutions[4].status != UNUSED.status))
		mexPrintSolutionSet(&solutions[4], j, k, l, m);

	solutions[5] = UNUSED;

	solutions[6] = ((Aprev > 0) && (solutions[2].H1 < EPS_ZERO)) ? foc_int_solve_H1_H2(eqCalc, kttp1, L1_star, L2_star, A_star, Agrid2, amin) : UNUSED;
	if (printSols && (solutions[6].status != UNUSED.status))
		mexPrintSolutionSet(&solutions[6], j, k, l, m);

	solutions[7] = foc_int_solve_A_H1_H2(eqCalc);
	if (printSols && (solutions[7].status != UNUSED.status))
		mexPrintSolutionSet(&solutions[7], j, k, l, m);

	//Select best corner (according to V):
	for (int sol_index = 0; sol_index < 8; sol_index++)
	{
		if (res.V < solutions[sol_index].V)
			res = solutions[sol_index];
	}

	if (res.V == -DBL_MAX)
	{
		res.CorIndex = sol_corner::error_cor;
	}
	

	res.U = eqCalc->calc_U_from_C(res.C);

	if (printSols)
	{ 
		mexPrintf("The final result is: \n");
		mexPrintSolutionSet(&res, j, k, l, m);
		mexPrintf("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n");
	}

	return res;
}

SolutionSet foc_Solve_Single_1(OptimizationParams * oParams, EquationCalculator * eqCalc, int j, int k, int l, int m, int amin, int Aprev, int Agrid2)
{	
	int printSols = oParams->Get_print_level() != printLevel::enum_printLevel_unknown;

	if (printSols)
		mexPrintf("~~~~~~~~ CURRENTLY SOLVING (%i,%i,%i,%i): ~~~~~~~~\n", j, k, l, m);

	DoublePair fval, LT, LTres;
	SolutionSet solutions[8];
	SolutionSet res;

	SolutionSet UNUSED;
	UNUSED.V = -DBL_MAX;
	UNUSED.status = -3;

	eqCalc->SetLoopState(oParams, j, k, l, m);
	double L1_star = oParams->Get_L1_star_f_val(j, k, l, m);
	double L2_star = oParams->Get_L2_star_f_val(j, k, l, m);
	double T1_star = oParams->Get_T1_star_f_val(j, k, l, m);
	double A_star = oParams->Get_A_star_f_val(j, k, l, m);
	int kttp1 = eqCalc->GetKttp();

	solutions[0] = foc_int_solve_std(eqCalc, kttp1, L1_star, T1_star, amin);
	res = solutions[0];

	if (printSols)
		mexPrintSolutionSet(&solutions[0], j, k, l, m);

	if (res.H1 < EPS_ZERO || res.H2 < EPS_ZERO || res.A < amin)
	{
		res.V = -DBL_MAX;

		solutions[1] = solutions[0].A < amin ? foc_int_solve_A(eqCalc, kttp1, L1_star, T1_star) : UNUSED;
		if (printSols && (solutions[1].status != UNUSED.status))
			mexPrintSolutionSet(&solutions[1], j, k, l, m);

		solutions[2] = UNUSED;

		solutions[3] = solutions[0].H1 < EPS_ZERO ? foc_int_solve_H1(eqCalc, kttp1, L2_star, amin) : UNUSED;
		if (printSols && (solutions[3].status != UNUSED.status))
			mexPrintSolutionSet(&solutions[3], j, k, l, m);

		solutions[4] = UNUSED;

		solutions[5] = (solutions[0].H1 < EPS_ZERO && (solutions[0].A < amin || solutions[3].A < amin)) ? foc_int_solve_A_H1(eqCalc, kttp1, L2_star, amin) : UNUSED;
		if (printSols && (solutions[5].status != UNUSED.status))
			mexPrintSolutionSet(&solutions[5], j, k, l, m);

		solutions[6] = UNUSED;

		solutions[7] = UNUSED;

		for (int sol_index = 1; sol_index < 8; sol_index++)
		{
			if (res.V < solutions[sol_index].V)
				res = solutions[sol_index];
		}

		if (res.V == -DBL_MAX)
		{
			res.CorIndex = sol_corner::error_cor;
		}
	}

	res.U = eqCalc->calc_U_from_C(res.C);

	if (printSols)
	{ 
		mexPrintf("The final result is: \n");
		mexPrintSolutionSet(&res, j, k, l, m);
		mexPrintf("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n");
	}

	return res;
}

SolutionSet foc_int_solve_std(EquationCalculator * eqCalc, int kttp1, double L1_star, double T1_star, double amin)
{
	SolutionSet res;
	DoublePair fval, LT, LTres;

	eqCalc->SetNormSwitch(0);
	LT.eqCalc = eqCalc;
	LT.x0 = L1_star;
	LT.x1 = T1_star + 0.01;
	
	if (kttp1 == 2)
	{
		LT.x0 = 0.9*L1_star;
		LT.x1 = 0.9*T1_star + 0.01;
	}

	LTres = solve_single_foc_double_op(LT, EquationCalculator::foc_int_std);
	fval = EquationCalculator::foc_int_std(LTres);
	double sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;

	if (sqRes > pow(10, -5))
	{
		eqCalc->SetNormSwitch((abs(fval.x0) > abs(fval.x1)) ? 1 : 2);

		LTres = solve_single_foc_double_op(LT, EquationCalculator::foc_int_std);
		fval = EquationCalculator::foc_int_std(LTres);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;
	}

	res = eqCalc->CalcFullSolutionSetStd(LTres);
	res.F = sqRes;
	res.status = LTres.status;

	if (res.F > pow(10, -6))
	{
		res.H1 = -1;
		res.H2 = -1;
		res.A = amin - 1;
	}

	return res;
}

SolutionSet foc_int_solve_A(EquationCalculator * eqCalc, int kttp1, double L1_star, double T1_star)
{
	//The Declarations:
	SolutionSet res;
	DoublePair fval, LT, LTres;

	eqCalc->SetNormSwitch(0);

	//Set default response value:
	double sqRes;

	//The Solutions:
	LT = { eqCalc, L1_star , T1_star + 0.01 };
	if (kttp1 == 2) {
		LT = { eqCalc, 0.9*L1_star , 0.9*T1_star + 0.01 };
	}

	LTres = solve_single_foc_double_op(LT, EquationCalculator::foc_cor_A);
	fval = EquationCalculator::foc_cor_A(LTres);
	sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;

	if (sqRes > pow(10, -5))
	{
		eqCalc->SetNormSwitch((abs(fval.x0) > abs(fval.x1)) ? 1 : 2);
		LTres = solve_single_foc_double_op(LT, EquationCalculator::foc_cor_A);
		fval = EquationCalculator::foc_cor_A(LTres);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;
	}
	LT = { eqCalc, 0.95 , 0.95 } ;
	for (int miter = 1; sqRes > pow(10, -5) && miter < 11; miter++)
	{
		LTres = solve_single_foc_double_op(LT, EquationCalculator::foc_cor_A);
		fval = EquationCalculator::foc_cor_A(LTres);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;
		LT.x0 = (LT.x0 - 0.1);
		LT.x1 = (LT.x1 - 0.1);
		if (LT.x0<0.01)
		{
			LT.x0 = 0.01;
		}
		if (LT.x1<0.01)
		{
			LT.x1 = 0.01;
		}
	}

	res = eqCalc->CalcFullSolutionSetA(LTres);
	res.F = sqRes;
	res.status = LTres.status;

	if (res.H1 < EPS_ZERO || res.H2 < EPS_ZERO || sqRes > pow(10, -5))
		res.V = -DBL_MAX;

	return res;
}

SolutionSet foc_int_solve_H2(EquationCalculator * eqCalc, int kttp1, double L1_star, double amin)
{
	//The Declarations:
	SolutionSet res;
	DoublePair fval, L1LAM2, L1LAM2res;

	eqCalc->SetNormSwitch(0);

	//Set default response value:
	double sqRes;
	L1LAM2.eqCalc = eqCalc;
	L1LAM2.x0 = L1_star;
	L1LAM2.x1 = 0; 
	if (kttp1 == 2)
	{
		L1LAM2.x0 = 0.9*L1_star;
		L1LAM2.x1 = 0; 
	}

	L1LAM2res = solve_single_foc_double_op(L1LAM2, EquationCalculator::foc_cor_h2);;
	fval = EquationCalculator::foc_cor_h2(L1LAM2res);
	sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;

	if (sqRes > pow(10, -5))
	{
		eqCalc->SetNormSwitch((abs(fval.x0) > abs(fval.x1)) ? 1 : 2);
		L1LAM2res = solve_single_foc_double_op(L1LAM2, EquationCalculator::foc_cor_h2);
		fval = EquationCalculator::foc_cor_h2(L1LAM2res);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;
	}

	L1LAM2.x0 = 0.99;
	for (int miter = 1; sqRes > pow(10, -5) && miter < 101; miter++)
	{
		L1LAM2res = solve_single_foc_double_op(L1LAM2, EquationCalculator::foc_cor_h2);
		fval = EquationCalculator::foc_cor_h2(L1LAM2res);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;
		L1LAM2.x0 = (L1LAM2.x0 - 0.01);
		if (L1LAM2.x0<0.01)
		{ 
			L1LAM2.x0 = 0.01;
		}
	}

	res = eqCalc->CalcFullSolutionSetH2(L1LAM2res);
	res.F = sqRes;
	res.status = L1LAM2res.status;

	if (res.H1 < EPS_ZERO || res.A < amin || sqRes > pow(10, -5))
		res.V = -DBL_MAX;

	return res;
}

SolutionSet foc_int_solve_H1(EquationCalculator * eqCalc, int kttp1, double L2_star, double amin)
{
	//The Declarations:
	SolutionSet res;
	DoublePair fval, L2LAM1, L2LAM1res;
	eqCalc->SetNormSwitch(0);
	//Set default response value:
	double sqRes;

	L2LAM1.eqCalc = eqCalc;
	L2LAM1.x0 = L2_star;
	L2LAM1.x1 = 0; 
	if (kttp1 == 2)
	{ 
		L2LAM1.x0 = 0.9*L2_star;
		L2LAM1.x1 = 0; 
	}

	L2LAM1res = solve_single_foc_double_op(L2LAM1, EquationCalculator::foc_cor_h1);
	fval = EquationCalculator::foc_cor_h1(L2LAM1res);
	sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;

	if (sqRes > pow(10, -5))
	{
		eqCalc->SetNormSwitch((abs(fval.x0) > abs(fval.x1)) ? 1 : 2);
		L2LAM1res = solve_single_foc_double_op(L2LAM1, EquationCalculator::foc_cor_h1);
		fval = EquationCalculator::foc_cor_h1(L2LAM1res);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;
	}

	L2LAM1.x0 = 0.95;
	for (int miter = 1; sqRes > pow(10, -5) && miter < 11; miter++)
	{
		L2LAM1res = solve_single_foc_double_op(L2LAM1, EquationCalculator::foc_cor_h1);
		fval = EquationCalculator::foc_cor_h1(L2LAM1res);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;
		L2LAM1.x0 = (L2LAM1.x0 - 0.1);
		if (L2LAM1.x0<0.01)
		{
			L2LAM1.x0 = 0.01;
		}
	}

	res = eqCalc->CalcFullSolutionSetH1(L2LAM1res);
	res.F = sqRes;
	res.status = L2LAM1res.status;

	if (res.H2 < EPS_ZERO || res.A < amin || sqRes > pow(10, -5))
		res.V = -DBL_MAX;

	return res;
}

SolutionSet foc_int_solve_A_H2(EquationCalculator * eqCalc, int kttp1, double L1_star, double amin)
{
	//The Declarations:
	SolutionSet res;
	DoublePair fval, L1LAM2, L1LAM2res;

	eqCalc->SetNormSwitch(0);

	//Set default response value:
	double sqRes;
	L1LAM2.eqCalc = eqCalc;
	L1LAM2.x0 = L1_star;
	L1LAM2.x1 = 0; 
	if (kttp1 == 2)
	{
		L1LAM2.x0 = 0.9*L1_star;
		L1LAM2.x1 = 0; 
	}

	L1LAM2res = solve_single_foc_double_op(L1LAM2, EquationCalculator::foc_cor_A_h2);
	fval = EquationCalculator::foc_cor_A_h2(L1LAM2res);
	sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;

	if (sqRes > pow(10, -5))
	{
		eqCalc->SetNormSwitch((abs(fval.x0) > abs(fval.x1)) ? 1 : 2);
		L1LAM2res = solve_single_foc_double_op(L1LAM2, EquationCalculator::foc_cor_A_h2);
		fval = EquationCalculator::foc_cor_A_h2(L1LAM2res);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;
	}

	L1LAM2.x0 = 0.99;
	for (int miter = 1; sqRes > pow(10, -5) && miter < 101; miter++)
	{
		L1LAM2res = solve_single_foc_double_op(L1LAM2, EquationCalculator::foc_cor_A_h2);
		fval = EquationCalculator::foc_cor_A_h2(L1LAM2res);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;
		L1LAM2.x0 = (L1LAM2.x0 - 0.01);
		if (L1LAM2.x0<0.01)
		{
			L1LAM2.x0 = 0.01;
		}
	}

	res = eqCalc->CalcFullSolutionSetA_H2(L1LAM2res);
	res.F = sqRes;
	res.status = L1LAM2res.status;

	if (res.H1 < EPS_ZERO || sqRes > pow(10, -5))
		res.V = -DBL_MAX;

	return res;
}

SolutionSet foc_int_solve_A_H1(EquationCalculator * eqCalc, int kttp1, double L2_star, double amin)
{
	//The Declarations:
	SolutionSet res;
	DoublePair fval, L2LAM1, L2LAM1res;
	eqCalc->SetNormSwitch(0);
	//Set default response value:
	double sqRes;
	L2LAM1.eqCalc = eqCalc;
	L2LAM1.x0 = L2_star;
	L2LAM1.x1 = 0; 
	if (kttp1 == 2)
	{
		L2LAM1.x0 = 0.9*L2_star;
		L2LAM1.x1 = 0; 
	}

	L2LAM1res = solve_single_foc_double_op(L2LAM1, EquationCalculator::foc_cor_A_h1);
	fval = EquationCalculator::foc_cor_A_h1(L2LAM1res);
	sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;

	if (sqRes > pow(10, -5))
	{
		eqCalc->SetNormSwitch((abs(fval.x0) > abs(fval.x1)) ? 1 : 2);
		L2LAM1res = solve_single_foc_double_op(L2LAM1, EquationCalculator::foc_cor_A_h1);
		fval = EquationCalculator::foc_cor_A_h1(L2LAM1res);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;
	}

	L2LAM1.x0 = 0.95;
	for (int miter = 1; sqRes > pow(10, -5) && miter < 11; miter++)
	{
		L2LAM1res = solve_single_foc_double_op(L2LAM1, EquationCalculator::foc_cor_A_h1);
		fval = EquationCalculator::foc_cor_A_h1(L2LAM1res);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;
		L2LAM1.x0 = (L2LAM1.x0 - 0.1);
		if (L2LAM1.x0<0.01)
		{
			L2LAM1.x0 = 0.01;
		}
	}

	res = eqCalc->CalcFullSolutionSetA_H1(L2LAM1res);
	res.F = sqRes;
	res.status = L2LAM1res.status;

	if (res.H2 < EPS_ZERO || sqRes > pow(10, -5))
		res.V = -DBL_MAX;

	return res;
}

SolutionSet foc_int_solve_H1_H2(EquationCalculator * eqCalc, int kttp1, double L1_star, double L2_star, double A_star, double Agrid2, double amin)
{
	//The Declarations:
	SolutionSet res;
	DoubleTrio fval, L1L2A, L1L2Ares;
	eqCalc->SetNormSwitch(0);
	//Set default response value:
	int norm_switch = 0;
	double sqRes;

	L1L2A = { eqCalc, L1_star , L2_star , A_star };
	if (kttp1 == 2)
	{
		L1L2A.x0 = 0.9 * L1_star;
		L1L2A.x1 = 0.9 * L2_star;
		L1L2A.x2 = A_star;
	}

	L1L2Ares = solve_single_foc_trio_op(L1L2A, EquationCalculator::foc_cor_h1_h2);
	fval = EquationCalculator::foc_cor_h1_h2(L1L2Ares);
	sqRes = fval.x0*fval.x0 + fval.x1*fval.x1 + fval.x2*fval.x2;
	
	if (sqRes > pow(10, -5))
	{
		bool switch1 = abs(fval.x0) > 0.001;
		bool switch2 = abs(fval.x1) > 0.001;
		bool switch3 = abs(fval.x2) > 0.001;

		norm_switch = switch1 ? 1 : norm_switch;
		norm_switch = switch2 ? 2 : norm_switch;
		norm_switch = switch3 ? 3 : norm_switch;
		norm_switch = switch1 && switch2 ? 12 : norm_switch;
		norm_switch = switch1 && switch3 ? 13 : norm_switch;
		norm_switch = switch2 && switch3 ? 23 : norm_switch;
		norm_switch = switch1 && switch2 && switch3 ? 123 : norm_switch;

		eqCalc->SetNormSwitch(norm_switch);
		L1L2Ares = solve_single_foc_trio_op(L1L2A, EquationCalculator::foc_cor_h1_h2);
		fval = EquationCalculator::foc_cor_h1_h2(L1L2Ares);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1 + fval.x2*fval.x2;
	}

	if (sqRes > pow(10, -6))
	{
		L1L2A.x0 = 0.1;
		L1L2A.x1 = 0.1;
		L1L2A.x2 = Agrid2;
		L1L2Ares = solve_single_foc_trio_op(L1L2A, EquationCalculator::foc_cor_h1_h2);
		fval = EquationCalculator::foc_cor_h1_h2(L1L2Ares);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1 + fval.x2*fval.x2;
	}

	L1L2A.x0 = 0.95;
	L1L2A.x1 = 0.95;
	L1L2A.x2 = A_star;
	for (int miter = 1; sqRes > pow(10, -5) && miter < 11; miter++)
	{
		L1L2Ares = solve_single_foc_trio_op(L1L2A, EquationCalculator::foc_cor_h1_h2);
		fval = EquationCalculator::foc_cor_h1_h2(L1L2Ares);
		sqRes = fval.x0*fval.x0 + fval.x1*fval.x1 + fval.x2*fval.x2;
		L1L2A.x0 = (L1L2A.x0 - 0.1);
		L1L2A.x1 = (L1L2A.x0 - 0.1);
		if (L1L2A.x0<0.01)
		{
			L1L2A.x0 = 0.01;
		}
		if (L1L2A.x1<0.01)
		{
			L1L2A.x1 = 0.01;
		}
	}


	res = eqCalc->CalcFullSolutionSetH1_H2(L1L2Ares);
	res.F = sqRes;
	res.status = L1L2Ares.status;

	if (res.A < amin || res.C < EPS_ZERO || sqRes > pow(10, -5))
		res.V = -DBL_MAX;

	return res;
}

SolutionSet foc_int_solve_A_H1_H2(EquationCalculator * eqCalc)
{
	//The Declarations:
	SolutionSet res;
	DoublePair fval, L1L2, L1L2res;
	eqCalc->SetNormSwitch(0);
	//Set default response value:
	double sqRes;
	L1L2 = { eqCalc, 0.1, 0.1};

	L1L2res = solve_single_foc_double_op(L1L2, EquationCalculator::foc_cor_A_h1_h2);
	fval = EquationCalculator::foc_cor_A_h1_h2(L1L2res);
	sqRes = fval.x0*fval.x0 + fval.x1*fval.x1;

	res = eqCalc->CalcFullSolutionSetA_H1_H2(L1L2res);
	res.F = sqRes;
	res.status = L1L2res.status;
	if (sqRes > pow(10, -5))
		res.V = -DBL_MAX;

	return res;
}

SolutionSet foc_Solve_Single_0_nk(OptimizationParams * oParams, EquationCalculator * eqCalc, int j, int k, int l, int m, int amin, int Aprev, int Agrid2)
{
	int printSols = oParams->Get_print_level() != printLevel::enum_printLevel_unknown;

	if (printSols)
		mexPrintf("~~~~~~~~ CURRENTLY SOLVING (%i,%i,%i,%i): ~~~~~~~~\n", j, k, l, m);

	//Set repeat boundaries:
	DoublePair fval, LT, LTres;
	SolutionSet solutions[8];
	SolutionSet res;

	//Create a default solution set for "unused" corners
	SolutionSet UNUSED;
	UNUSED.V = -DBL_MAX;
	UNUSED.status = -3;

	eqCalc->SetLoopState(oParams, j, k, l, m);
	double L1_star = oParams->Get_L1_star_f_val(j, k, l, m);
	double L2_star = oParams->Get_L2_star_f_val(j, k, l, m);
	double T1_star = oParams->Get_T1_star_f_val(j, k, l, m);
	double A_star = oParams->Get_A_star_f_val(j, k, l, m);
	int kttp1 = eqCalc->GetKttp();

	//SolutionSet Calculations:

	res.V = -DBL_MAX;

	solutions[0] = UNUSED;

	solutions[1] = UNUSED;

	solutions[2] = foc_int_solve_H2_nk(eqCalc, kttp1, L1_star, amin);
	if (printSols && (solutions[2].status != UNUSED.status))
		mexPrintSolutionSet(&solutions[2], j, k, l, m);

	solutions[3] = UNUSED;

	solutions[4] = (solutions[2].A < amin) ? foc_int_solve_A_H2_nk(eqCalc, kttp1, L1_star, amin) : UNUSED;
	if (printSols && (solutions[4].status != UNUSED.status))
		mexPrintSolutionSet(&solutions[4], j, k, l, m);

	solutions[5] = UNUSED;

	solutions[6] = ((Aprev > 0) && (solutions[2].H1 < EPS_ZERO)) ? foc_int_solve_H1_H2_nk(eqCalc, kttp1, L1_star, L2_star, A_star, Agrid2, amin) : UNUSED;
	if (printSols && (solutions[6].status != UNUSED.status))
		mexPrintSolutionSet(&solutions[6], j, k, l, m);

	solutions[7] = foc_int_solve_A_H1_H2_nk(eqCalc);
	if (printSols && (solutions[7].status != UNUSED.status))
		mexPrintSolutionSet(&solutions[7], j, k, l, m);

	//Select best corner (according to V):

	for (int sol_index = 0; sol_index < 8; sol_index++)
	{
		if (res.V < solutions[sol_index].V)
			res = solutions[sol_index];
	}

	if (res.V == -DBL_MAX)
	{
		res.CorIndex = sol_corner::error_cor;
	}


	res.U = eqCalc->calc_U_from_C(res.C);

	if (printSols)
	{
		mexPrintf("The final result is: \n");
		mexPrintSolutionSet(&res, j, k, l, m);
		mexPrintf("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n");
	}

	return res;
}

SolutionSet foc_Solve_Single_1_nk(OptimizationParams * oParams, EquationCalculator * eqCalc, int j, int k, int l, int m, int amin, int Aprev, int Agrid2)
{
	int printSols = oParams->Get_print_level() != printLevel::enum_printLevel_unknown;

	if (printSols)
		mexPrintf("~~~~~~~~ CURRENTLY SOLVING (%i,%i,%i,%i): ~~~~~~~~\n", j, k, l, m);

	//Set repeat boundaries:
	DoublePair fval, LT, LTres;
	SolutionSet solutions[8];
	SolutionSet res;

	//Create a default solution set for "unused" corners
	SolutionSet UNUSED;
	UNUSED.V = -DBL_MAX;
	UNUSED.status = -3;

	eqCalc->SetLoopState(oParams, j, k, l, m);
	double L1_star = oParams->Get_L1_star_f_val(j, k, l, m);
	double L2_star = oParams->Get_L2_star_f_val(j, k, l, m);
	double T1_star = oParams->Get_T1_star_f_val(j, k, l, m);
	double A_star = oParams->Get_A_star_f_val(j, k, l, m);
	int kttp1 = eqCalc->GetKttp();

	//SolutionSet Calculations:
	solutions[0] = foc_int_solve_std_nk(eqCalc, kttp1, L1_star, T1_star, amin);
	res = solutions[0];

	if (printSols)
		mexPrintSolutionSet(&solutions[0], j, k, l, m);

	if (res.H1 < EPS_ZERO || res.H2 < EPS_ZERO || res.A < amin)
	{
		res.V = -DBL_MAX;

		solutions[1] = solutions[0].A < amin ? foc_int_solve_A_nk(eqCalc, kttp1, L1_star, T1_star) : UNUSED;
		if (printSols && (solutions[1].status != UNUSED.status))
			mexPrintSolutionSet(&solutions[1], j, k, l, m);

		solutions[2] = UNUSED;

		solutions[3] = solutions[0].H1 < EPS_ZERO ? foc_int_solve_H1_nk(eqCalc, kttp1, L2_star, amin) : UNUSED;
		if (printSols && (solutions[3].status != UNUSED.status))
			mexPrintSolutionSet(&solutions[3], j, k, l, m);

		solutions[4] = UNUSED;

		solutions[5] = (solutions[0].H1 < EPS_ZERO && (solutions[0].A < amin || solutions[3].A < amin)) ? foc_int_solve_A_H1_nk(eqCalc, kttp1, L2_star, amin) : UNUSED;
		if (printSols && (solutions[5].status != UNUSED.status))
			mexPrintSolutionSet(&solutions[5], j, k, l, m);

		solutions[6] = UNUSED;

		solutions[7] = UNUSED;

		//Select best corner (according to V):

		for (int sol_index = 1; sol_index < 8; sol_index++)
		{
			if (res.V < solutions[sol_index].V)
				res = solutions[sol_index];
		}

		if (res.V == -DBL_MAX)
		{
			res.CorIndex = sol_corner::error_cor;
		}
	}

	res.U = eqCalc->calc_U_from_C(res.C);

	if (printSols)
	{
		mexPrintf("The final result is: \n");
		mexPrintSolutionSet(&res, j, k, l, m);
		mexPrintf("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\n");
	}

	return res;
}

SolutionSet foc_int_solve_std_nk(EquationCalculator * eqCalc, int kttp1, double L1_star, double T1_star, double amin)
{
	SolutionSet res;
	DoubleSingle fval, LT, LTres;

	eqCalc->SetNormSwitch(0);
	LT.eqCalc = eqCalc;
	LT.x0 = L1_star;

	LTres = solve_single_foc_single_op(LT, EquationCalculator::foc_int_std_nk);
	fval = EquationCalculator::foc_int_std_nk(LTres);
	double sqRes = fval.x0*fval.x0;

	if (sqRes > pow(10, -5))
	{
		eqCalc->SetNormSwitch(1);

		LTres = solve_single_foc_single_op(LT, EquationCalculator::foc_int_std_nk);
		fval = EquationCalculator::foc_int_std_nk(LTres);
		sqRes = fval.x0*fval.x0;
	}

	res = eqCalc->CalcFullSolutionSetStd_nk(LTres);
	res.F = sqRes;
	res.status = LTres.status;

	if (res.F > pow(10, -6))
	{
		res.H1 = -1;
		res.H2 = -1;
		res.A = amin - 1;
	}

	return res;
}

SolutionSet foc_int_solve_A_nk(EquationCalculator * eqCalc, int kttp1, double L1_star, double T1_star)
{
	//The Declarations:
	SolutionSet res;
	DoubleSingle fval, LT, LTres;

	eqCalc->SetNormSwitch(0);

	//Set default response value:
	double sqRes;

	//The Solutions:
	LT.eqCalc = eqCalc;
	LT.x0 = L1_star;

	LTres = solve_single_foc_single_op(LT, EquationCalculator::foc_cor_A_nk);
	fval = EquationCalculator::foc_cor_A_nk(LTres);
	sqRes = fval.x0*fval.x0;

	if (sqRes > pow(10, -5))
	{
		eqCalc->SetNormSwitch(1);
		LTres = solve_single_foc_single_op(LT, EquationCalculator::foc_cor_A_nk);
		fval = EquationCalculator::foc_cor_A_nk(LTres);
		sqRes = fval.x0*fval.x0;
	}

	res = eqCalc->CalcFullSolutionSetA_nk(LTres);
	res.F = sqRes;
	res.status = LTres.status;

	if (res.H1 < EPS_ZERO || res.H2 < EPS_ZERO || sqRes > pow(10, -5))
		res.V = -DBL_MAX;

	return res;
}

SolutionSet foc_int_solve_H2_nk(EquationCalculator * eqCalc, int kttp1, double L1_star, double amin)
{
	//The Declarations:
	SolutionSet res;
	DoubleSingle fval, L1LAM2, L1LAM2res;

	eqCalc->SetNormSwitch(0);

	//Set default response value:
	double sqRes;
	L1LAM2.eqCalc = eqCalc;
	L1LAM2.x0 = L1_star;

	L1LAM2res = solve_single_foc_single_op(L1LAM2, EquationCalculator::foc_cor_h2_nk);;
	fval = EquationCalculator::foc_cor_h2_nk(L1LAM2res);
	sqRes = fval.x0*fval.x0;

	if (sqRes > pow(10, -5))
	{
		eqCalc->SetNormSwitch(1);
		L1LAM2res = solve_single_foc_single_op(L1LAM2, EquationCalculator::foc_cor_h2_nk);
		fval = EquationCalculator::foc_cor_h2_nk(L1LAM2res);
		sqRes = fval.x0*fval.x0;
	}

	res = eqCalc->CalcFullSolutionSetH2_nk(L1LAM2res);
	res.F = sqRes;
	res.status = L1LAM2res.status;

	if (res.H1 < EPS_ZERO || res.A < amin || sqRes > pow(10, -5))
		res.V = -DBL_MAX;

	return res;
}

SolutionSet foc_int_solve_H1_nk(EquationCalculator * eqCalc, int kttp1, double L2_star, double amin)
{
	//The Declarations:
	SolutionSet res;
	DoubleSingle fval, L2LAM1, L2LAM1res;
	eqCalc->SetNormSwitch(0);
	//Set default response value:
	double sqRes;

	L2LAM1.eqCalc = eqCalc;
	L2LAM1.x0 = L2_star;

	L2LAM1res = solve_single_foc_single_op(L2LAM1, EquationCalculator::foc_cor_h1_nk);
	fval = EquationCalculator::foc_cor_h1_nk(L2LAM1res);
	sqRes = fval.x0*fval.x0;

	if (sqRes > pow(10, -5))
	{
		eqCalc->SetNormSwitch(1);
		L2LAM1res = solve_single_foc_single_op(L2LAM1, EquationCalculator::foc_cor_h1_nk);
		fval = EquationCalculator::foc_cor_h1_nk(L2LAM1res);
		sqRes = fval.x0*fval.x0;
	}

	res = eqCalc->CalcFullSolutionSetH1_nk(L2LAM1res);
	res.F = sqRes;
	res.status = L2LAM1res.status;

	if (res.H2 < EPS_ZERO || res.A < amin || sqRes > pow(10, -5))
		res.V = -DBL_MAX;

	return res;
}

SolutionSet foc_int_solve_A_H2_nk(EquationCalculator * eqCalc, int kttp1, double L1_star, double amin)
{
	//The Declarations:
	SolutionSet res;
	DoubleSingle fval, L1LAM2, L1LAM2res;

	eqCalc->SetNormSwitch(0);

	//Set default response value:
	double sqRes;
	L1LAM2.eqCalc = eqCalc;
	L1LAM2.x0 = L1_star;

	L1LAM2res = solve_single_foc_single_op(L1LAM2, EquationCalculator::foc_cor_A_h2_nk);
	fval = EquationCalculator::foc_cor_A_h2_nk(L1LAM2res);
	sqRes = fval.x0*fval.x0;

	if (sqRes > pow(10, -5))
	{
		eqCalc->SetNormSwitch(1);
		L1LAM2res = solve_single_foc_single_op(L1LAM2, EquationCalculator::foc_cor_A_h2_nk);
		fval = EquationCalculator::foc_cor_A_h2_nk(L1LAM2res);
		sqRes = fval.x0*fval.x0;
	}

	res = eqCalc->CalcFullSolutionSetA_H2_nk(L1LAM2res);
	res.F = sqRes;
	res.status = L1LAM2res.status;

	if (res.H1 < EPS_ZERO || sqRes > pow(10, -5))
		res.V = -DBL_MAX;

	return res;
}

SolutionSet foc_int_solve_A_H1_nk(EquationCalculator * eqCalc, int kttp1, double L2_star, double amin)
{
	//The Declarations:
	SolutionSet res;
	DoubleSingle fval, L2LAM1, L2LAM1res;
	eqCalc->SetNormSwitch(0);
	//Set default response value:
	double sqRes;

	L2LAM1.eqCalc = eqCalc;
	L2LAM1.x0 = L2_star;

	L2LAM1res = solve_single_foc_single_op(L2LAM1, EquationCalculator::foc_cor_A_h1_nk);
	fval = EquationCalculator::foc_cor_A_h1_nk(L2LAM1res);
	sqRes = fval.x0*fval.x0;

	if (sqRes > pow(10, -5))
	{
		eqCalc->SetNormSwitch(1);
		L2LAM1res = solve_single_foc_single_op(L2LAM1, EquationCalculator::foc_cor_A_h1_nk);
		fval = EquationCalculator::foc_cor_A_h1_nk(L2LAM1res);
		sqRes = fval.x0*fval.x0;
	}

	res = eqCalc->CalcFullSolutionSetA_H1_nk(L2LAM1res);
	res.F = sqRes;
	res.status = L2LAM1res.status;

	if (res.H2 < EPS_ZERO || sqRes > pow(10, -5))
		res.V = -DBL_MAX;

	return res;
}

SolutionSet foc_int_solve_H1_H2_nk(EquationCalculator * eqCalc, int kttp1, double L1_star, double L2_star, double A_star, double Agrid2, double amin)
{
	//The Declarations:
	SolutionSet res;
	DoubleSingle fval, L1L2A, L1L2Ares;
	eqCalc->SetNormSwitch(0);
	//Set default response value:
	int norm_switch = 0;
	double sqRes;

	L1L2A.eqCalc = eqCalc;
	L1L2A.x0 =  A_star ;

	L1L2Ares = solve_single_foc_single_op(L1L2A, EquationCalculator::foc_cor_h1_h2_nk);
	fval = EquationCalculator::foc_cor_h1_h2_nk(L1L2Ares);
	sqRes = fval.x0*fval.x0;

	if (sqRes > pow(10, -5))
	{
		eqCalc->SetNormSwitch(1);
		L1L2Ares = solve_single_foc_single_op(L1L2A, EquationCalculator::foc_cor_h1_h2_nk);
		fval = EquationCalculator::foc_cor_h1_h2_nk(L1L2Ares);
		sqRes = fval.x0*fval.x0;
	}

	res = eqCalc->CalcFullSolutionSetH1_H2_nk(L1L2Ares);
	res.F = sqRes;
	res.status = L1L2Ares.status;

	if (res.A < amin || res.C < EPS_ZERO || sqRes > pow(10, -5))
		res.V = -DBL_MAX;

	return res;
}

SolutionSet foc_int_solve_A_H1_H2_nk(EquationCalculator * eqCalc)
{
	//Note that no optimization here
	SolutionSet res;
	res = eqCalc->CalcFullSolutionSetA_H1_H2_nk();
	return res;
}