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

int n = 101;				// Number of Grid Points
Vector<double> Grid1;		// Grid
const double cBar = 0.2;
const double Beta = 1.0;
const double p = 0.75;		// Probability that Country 1 Wins
const double Alpha = 0.5;	// Probability that Country 2 Wins


int Func1(const Vector<double> &x, Vector<double> &xNew)
{
	Vector<double> x1(n);
	Vector<double> x2(n);
	for(int i=0; i<n; i++)
	{
		x1(i) = x(i);
		x2(i) = x(i+n);
	}

	Vector<double> x1New(n);
	Vector<double> x2New(n);

	/* Apply T1 */
	for(int i=0; i<n; i++) // For Each c1
	{
		double MaxUtil = -Inf;
		int MaxIndex = -1;

		for(int j=0; j<n; j++) // For Each Possible Maximizer
		{
			double CurrUtil = Zero;
			double FDiff = cBar / (double)n;
			for(int k=0; k<n; k++) // Sum Utilities
			{
				if(Grid1(j) >= x2(k)) CurrUtil += (Alpha * Grid1(j) + (One - Alpha) * x2(k)) * FDiff;
				else                  CurrUtil += (Beta * p - Grid1(i)) * FDiff;
			}
			if(CurrUtil > MaxUtil)
			{
				MaxUtil = CurrUtil;
				MaxIndex = j;
			}
		}
		x1New(i) = Grid1(MaxIndex);
	}

	/* Apply T2 */
	for(int i=0; i<n; i++) // For Each c2
	{
		double MaxUtil = -Inf;
		int MaxIndex = -1;

		for(int j=0; j<n; j++) // For Each Possible Maximizer
		{
			double CurrUtil = Zero;
			double FDiff = cBar / (double)n;
			for(int k=0; k<n; k++) // Sum Utilities
			{
				if(x1(k) >= Grid1(j)) CurrUtil += (One - Alpha * x1(k) - (One - Alpha) * Grid1(j)) * FDiff;
				else                  CurrUtil += (Beta * (One - p) - Grid1(i)) * FDiff;
			}
			if(CurrUtil > MaxUtil)
			{
				MaxUtil = CurrUtil;
				MaxIndex = j;
			}
		}
		x2New(i) = Grid1(MaxIndex);
	}

	for(int i=0; i<n; i++)
	{
		xNew(i)   = x1New(i);
		xNew(i+n) = x2New(i);
	}

	return 0;

}

int Func2(const Vector<double> &x, Vector<double> &xNew)
{
	Vector<double> x1(n);
	Vector<double> x2(n);
	for(int i=0; i<n; i++)
	{
		x1(i) = x(i);
		x2(i) = x(i+n);
	}

	Vector<double> x1New(n);
	Vector<double> x2New(n);

	/* Apply T1 */
	for(int i=0; i<n; i++) // For Each c1
	{
		double MaxUtil = -Inf;
		int MaxIndex = -1;
		for(int j=0; j<n; j++) // For Each Possible Maximizer
		{
			double CurrUtil = Zero;
			for(int k=0; k<n; k++)
			{
				double FDiff = One / (double)(n - 1);
			    CurrUtil += (One - Grid1(j) - x2(k) - Grid1(i)) * Grid1(j) * FDiff;
			}
			if(CurrUtil > MaxUtil)
			{
				MaxUtil = CurrUtil;
				MaxIndex = j;
			}
		}
		x1New(i) = Grid1(MaxIndex);
	}

	/* Apply T2 */
	for(int i=0; i<n; i++) // For Each c2
	{
		double MaxUtil = -Inf;
		int MaxIndex = -1;
		for(int j=0; j<n; j++) // For Each Possible Maximizer
		{
			double CurrUtil = Zero;
			for(int k=0; k<n; k++)
			{
				double FDiff = One / (double)(n - 1);
			    CurrUtil += (One - Grid1(j) - x1(k) - Grid1(i)) * Grid1(j) * FDiff;
			}
			if(CurrUtil > MaxUtil)
			{
				MaxUtil = CurrUtil;
				MaxIndex = j;
			}
		}
		x2New(i) = Grid1(MaxIndex);
	}


	for(int i=0; i<n; i++)
	{
		xNew(i)   = x1New(i);
		xNew(i+n) = x2New(i);
	}

	return 0;
}

int Func3(const Vector<double> &x, Vector<double> &f)
{
	Vector<double> x1(n);
	Vector<double> x2(n);
	for(int i=0; i<n; i++)
	{
		x1(i) = x(i);
		x2(i) = x(i+n);
	}

	Vector<double> x1New(n);
	Vector<double> x2New(n);

	/* Apply T1 */
	for(int i=0; i<n; i++) // For Each c1
	{
		double MaxUtil = -Inf;
		int MaxIndex = -1;
		for(int j=0; j<n; j++) // For Each Possible Maximizer
		{
			double CurrUtil = Zero;
			for(int k=0; k<n; k++)
			{
				double FDiff = One / (double)(n - 1);
			    CurrUtil += (One - Grid1(j) - x2(k) - Grid1(i)) * Grid1(j) * FDiff;
			}
			if(CurrUtil > MaxUtil)
			{
				MaxUtil = CurrUtil;
				MaxIndex = j;
			}
		}
		x1New(i) = Grid1(MaxIndex);
	}

	/* Apply T2 */
	for(int i=0; i<n; i++) // For Each c2
	{
		double MaxUtil = -Inf;
		int MaxIndex = -1;
		for(int j=0; j<n; j++) // For Each Possible Maximizer
		{
			double CurrUtil = Zero;
			for(int k=0; k<n; k++)
			{
				double FDiff = One / (double)(n - 1);
			    CurrUtil += (One - Grid1(j) - x1(k) - Grid1(i)) * Grid1(j) * FDiff;
			}
			if(CurrUtil > MaxUtil)
			{
				MaxUtil = CurrUtil;
				MaxIndex = j;
			}
		}
		x2New(i) = Grid1(MaxIndex);
	}


	for(int i=0; i<n; i++)
	{
		f(i)   = x1(i) - x1New(i);
		f(i+n) = x2(i) - x2New(i);
	}

	return 0;

}

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

	Grid1 = Grid(Zero,One,n);
	Grid1.Name = "Grid1";

	//SolveFixed sf(Func1,2*n);
	//Set(sf.x,0.5);
	//sf.Lambda = 0.1;
	//sf.MaxIter = 1000;
	//sf.Solve();

	SolveNewton< Matrix<double> > sf(Func3,2*n);
	//SolveNelderMead sf(Func3,2*n);
	sf.Solve();

	Vector<double> x1(n);	x1.Name = "x1";
	Vector<double> x2(n);	x2.Name = "x2";

	for(int i=0; i<n; i++)
	{
		x1(i) = sf.x(i);
		x2(i) = sf.x(i+n);
	}

	Output("x.dat",Grid1,x1,x2);

	Print("End");
	return 0;
}