#include "MpUtil.h"
#include "NashEquilibriumMulti.cpp"

int N = 5001;
int D = 2;
int J = 4;

Vector<double> Lambda(J);
Matrix<double> v(N,D);


int Func1(const Vector< Vector<double> > &x, Vector<double> &Util)
{
	Set(Util,Zero);
	Vector<double> Temp(J);
	for(int n=0; n<N; n++)
	{
		double Den = Zero;
		for(int j=0; j<J; j++)
		{
			Temp(j) = exp(Lambda(j) - Sqr(v(n,0) - x(j)(0)) - Sqr(v(n,1) - x(j)(1)));
			Den += Temp(j);
		}

		for(int j=0; j<J; j++) Util(j) += (Temp(j) / Den) / (double)N;
	}

	return 0;
}

int Func2(const Vector<double> &x, Vector<double> &xNew)
{

	Vector<double> Temp(J);
	Matrix<double> Num(J,D,Zero);
	Vector<double> Den(J,Zero);

	for(int n=0; n<N; n++)
	{
		double Sm = Zero;
		for(int j=0; j<J; j++)
		{
			Temp(j) = exp(Lambda(j) - Sqr(v(n,0) - x(D*j)) - Sqr(v(n,1) - x(D*j+1)));
			Sm += Temp(j);
		}

		for(int j=0; j<J; j++)
		{
			double q = Temp(j) / Sm;
			for(int d=0; d<D; d++) Num(j,d) += v(n,d) * q * (One - q);
			Den(j) += q * (One - q);
		}
	}

	int Curr = 0;
	for(int j=0; j<J; j++) for(int d=0; d<D; d++) xNew(Curr++) = Num(j,d) / Den(j);
	return 0;
}

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

	long RandomSeed = -23987324;

	for(int n=0; n<N; n++)
	{
		v(n,0) = NormalSample(0.0,2.0,RandomSeed);
		v(n,1) = NormalSample(0.0,0.5,RandomSeed);
	}

	Lambda(0) = 2.0;
	Lambda(1) = 0.0;
	Lambda(2) = 0.0;
	Lambda(3) = 2.0;

	NashEquilibriumMulti ne(Func1,Vector<int>(J,2));
	ne.MaxIter = 100;
	ne.Lambda = 0.25;
	ne.Method = 2;
	ne.Option = 2;
	//ne.Solve();
	ne.PrintLevel = 0;
	//ne.Option = 1;
	//ne.Solve();
	//Print("ne.x",ne.x);

	Print("ne.Check()",ne.Check());
	Print("ne.CheckLocal()",ne.CheckLocal());


	SolveFixed sf(Func2,D*J);

	for(int i=1; i<=100; i++)
	{
		Vector<double> Temp = RandomNormalVector(D*J,RandomSeed);
		Copy(sf.x,Temp);
		sf.PrintLevel = 0;
		sf.Solve();
		for(int j=0; j<J; j++) for(int d=0; d<D; d++) ne.x(j)(d) = sf.x(D*j+d);
		Print("Iter",i,"ne.CheckLocal()",ne.CheckLocal(),"ne.Check()",ne.Check(),"sf.x",sf.x);
	}

	Print("End");
	return 0;
}