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

int N = 3;		// Number of Legislators
int J = 121;	// Size of Grid

double q;
double Beta = 0.99;
Vector<double> p(N);
Vector<double> w(N);
Vector<bool> vStar(J);
Matrix<double> VStar(N,N);
Matrix<double> U(N,J);
Vector<double> bGrid;
Matrix<bool> App(N,J);

string PrintSet(const Vector<bool> &Set1, const Vector<double> &Grid1)
{
	string s = "";

	int K = Set1.Size();

	if(Grid1.Size() != K)
	{
		Print("oiuwer");
		HaltExecution();
	}

	bool Started = 0;
	for(int k=0; k<K; k++)
	{
		if(Started)
		{
			if(Set1(k) == 0)
			{
				s += ToString(Grid1(k-1)) + "]";
				Started = 0;
			}
		}
		else
		{
			if(Set1(k) == 1)
			{
				s += "[" + ToString(Grid1(k)) + ",";
				Started = 1;
			}
		}
	}
	if(Set1(K-1) == 1 && Started == 1)
	{
		s += ToString(Grid1(K-1)) + "]";
	}

	return s;
}

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

	/* Reform V and VNew */
	Vector<double> xCopy = x.Copy();

	Matrix<double> V(N,J,&xCopy[0]);
	Matrix<double> VNew(N,J,&xNew[0]);

	/* Precompute for Efficiency */
	for(int n=0; n<N; n++)
	{
		VStar(n,0) = V(n,40);
		VStar(n,1) = V(n,60);
		VStar(n,2) = V(n,80);
	}

	Vector< Vector<bool> > vnStar(N);
	for(int n=0; n<N; n++) vnStar(n) = Vector<bool>(J);

	/*
	for(int n=0; n<N; n++)
	{
		double Temp = Zero;
		for(int m=0; m<N; m++) Temp += p(m) * VStar(n,m);
		Temp *= Beta;
		//ContUtil(n) = Temp;
	}
	*/

	/* Calculate vStar */
	Vector<double> Votes(J);
	for(int j=0; j<J; j++)
	{
		Votes(j) = Zero;
		for(int n=0; n<N; n++)
		{
			double Temp = Zero;
			for(int m=0; m<N; m++) Temp += p(m) * VStar(n,m);
			Temp *= Beta;
			if(U(n,j) >= Temp)
			{
				vnStar(n)(j) = True;
				Votes(j) += w(n);
			}
			else
			{
				vnStar(n)(j) = False;
			}
		}
		if(Votes(j) >= q) vStar(j) = True;
		else              vStar(j) = False;
	}

	Print("vStar"    ,PrintSet(vStar,bGrid    ));
	Print("vnStar(0)",PrintSet(vnStar(0),bGrid));
	Print("vnStar(1)",PrintSet(vnStar(1),bGrid));
	Print("vnStar(2)",PrintSet(vnStar(2),bGrid));
	//PauseExecution();


	/* Update VStar */
	for(int n=0; n<N; n++) for(int j=0; j<J; j++)
	{
		double Temp = Zero;
		for(int m=0; m<N; m++) Temp += p(m) * VStar(n,m);
		Temp *= Beta;

		/* Vote for the proposal */
		double Votesm1 = Votes(j);
		if(vnStar(n)(j) == True) Votesm1 -= w(n);

		double Util1 = Temp;
		if(Votesm1 + w(n) >= q) Util1 = U(n,j);

		double Util2 = Temp;
		if(Votesm1 >= q) Util2 = U(n,j);

		VNew(n,j) = Max(Util1,Util2);
	}

	/*
	Vector<bool> Pass(N);
	Vector<double> Proposal(N);
	for(int n=0; n<N; n++)
	{
		if(vStar(j) == One) Pass(n) = True;
		else                Pass(n) = False;

		Proposal(n) = bGrid(j);
	}
	Print("Pass",Pass);
	Print("Proposal",Proposal);
	*/

	return 0;
}



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

	q = 0.50000001; // Majority Rule
	w.Name = "w";
	for(int n=0; n<N; n++) w(n) = One / (double)N; // Equal Voting Power
	p.Name = "p";
	for(int n=0; n<N; n++) p(n) = One / (double)N; // Equal Proposal Power

	/* Compute Policy Utilities on the Grid */
	Vector<double> Alpha = LoadVector<double>(N,"-1,0,1");
	long RandomSeed = -234098234;
	//Vector<double> Alpha = RandomNormalVector(N,RandomSeed);
	bGrid = Grid(-3.0,3.0,J);
	for(int n=0; n<N; n++) for(int j=0; j<J; j++) U(n,j) = 16.0 - Abs(Alpha(n) - bGrid(j));

	SolveFixed sf(Func2,N*J);
	sf.Lambda = 1.0;

	for(int n=0; n<N; n++) for(int j=0; j<J; j++) sf.x(J*n+j) = 0.0;

	sf.MaxIter = 5000;
	sf.Lambda = 0.1;
	sf.Solve();

	Vector<double> xCopy = sf.x.Copy();

	Matrix<double> V    (N,J,&xCopy[0]  );

	/*
	Vector<bool> Pass(N);
	Vector<double> Outcome(N);

	for(int n=0; n<N; n++)
	{
		int j = round(bStar(n));
		Print("j",j,"bGrid(j)",bGrid(j));
		if(abs(vStar(j) - One) < 0.0001) Pass(n) = True;
		else                             Pass(n) = False;

		Outcome(n) = bGrid(j);
	}

	//Print("App",App);
	Print("Alpha",Alpha);
	Print("Pass",Pass);
	Print("Outcome",Outcome);

	Output("vstar.dat",bGrid,vStar);
	Output("dat1.dat",bGrid,Transpose(App));
	*/

	Print("End");
	return 0;
}