#include "MpUtil.h"
#include "Interpolate.cpp"
#include "SolveFixed.cpp"

int M = 201;			// Number of Grid Points
double mBar = 10.0;		// Grid Upper Bound
Vector<double> mGrid;
Vector<double> Policy;
double d = 1.0;
double Beta = 0.90;
double r = 0.05;
double Gamma1 = 0.6;
double Gamma0 = 0.4;

int Func1(const Vector<double> &x, Vector<double> &y)
{

	for(int m1=0; m1<M; m1++)
	{
		double MaxUtil = Gamma1 - (Gamma1 - Gamma0) * exp(-mGrid(0)) + Beta * InterpolateLinear((One + r) * (x(m1) + d - mGrid(0)),mGrid,x);
		if(mGrid(0) > mGrid(m1) + d) MaxUtil = -Inf;	// Impose Constraint

		double MaxIndex = 0;
		for(int m2=1; m2<M; m2++)
		{
			double Util = Gamma1 - (Gamma1 - Gamma0) * exp(-mGrid(m2)) + Beta * InterpolateLinear((One + r) * (x(m1) + d - mGrid(m2)),mGrid,x);
			if(mGrid(m2) > mGrid(m1) + d) Util = -Inf;	// Impose Constraint

			if(Util > MaxUtil)
			{
				MaxUtil = Util;
				MaxIndex = m2;
			}
		}

		y(m1) = MaxUtil;
		Policy(m1) = mGrid(MaxIndex);
	}

	return 0;

}

int main()
{
	Print("Begin");

	mGrid = Grid(Zero,mBar,M);
	mGrid.Name = "x";
	Policy = Vector<double>(M);
	Policy.Name = "Policy";

	SolveFixed sf(Func1,M);
	sf.MaxIter = 5000;
	sf.Solve();
	sf.x.Name = "Value";

	Output("plot1.dat",mGrid,sf.x,Policy);

	Print("End");
	return 0;
}