//=====================================================
//Shortest path on a grid, Dynamic Programming / Djikstra
// Find the shortest path from one grid cell to another.
//=======================================================


#include <iostream>
#include <algorithm>

using namespace std;

const int N = 5;

// print out 2D array
void printMatrix(int arr[][N])
{

	for(int i = 0; i < N; i++)
	{		
		for(int j = 0; j < N; j++)
		{
			cout<<"\t"<<arr[i][j]<<" ";
		}	
		cout<<endl;
	}
}


bool areEqualArrays(int a[][N], int b[][N])
{
	for(int i = 0; i < N; i++)
	{
		for(int j = 0; j < N; j++)
		{
			if(a[i][j] != b[i][j])
				return false;
		}
	}

	return true;
}

// return best cost to go from neighbor from i,j
int minNeighborCostToGo(int C[][N], int r, int c, int &mr, int &mc)
{
	int minCost = C[r][c];

	if(r > 0)
	{
		if(C[r-1][c] < minCost)
		{
			mr = r-1;
			mc = c;
			minCost = C[r-1][c];	
		}
	}
	if(c > 0)
	{
		if(C[r][c-1] < minCost)
		{
			minCost = C[r][c-1];
			mr = r;
			mc = c-1;
		}
	}
	if(r < N-1)
	{
		if(C[r+1][c] < minCost)
		{
			minCost = C[r+1][c];
			mr = r+1;
			mc = c;
		}
	}
	if (c < N - 1)
	{
		if(C[r][c+1] < minCost)
		{
			minCost = C[r][c+1];
			mr = r;
			mc = c + 1 ;
		}
	}

	return minCost;
}

int main()
{

	// define starting position
	int startc = 0;
	int startr = 0;

	// define goal position
	int goalc = N-1;
	int goalr = N-2;

	// define cost matrix
	int C[N][N];

	// set initial cost to 1
	fill(&C[0][0], &C[N-1][N-1], 1);

	// cost to go from goal is 0
	C[goalr][goalc] = 0;

	int obsr = 2;
	int obsc = 2;

	C[obsr][obsc] = 100;

	bool converged = false;

	int Ctemp[N][N];

	int mr, mc;

	while(!converged)
	{	

		// copy matrix from C to Ctemp
		copy(&C[0][0], &C[N-1][N-1]+1,&Ctemp[0][0]);

		// loop and update cost matrix
		for(int i = 0; i < N; i++)
		{
			for(int j = 0; j < N; j++)
			{
				if(i == goalr && j == goalc)
				{				
					C[i][j] = 0;
				}
				if(i == obsr && j == obsc)
					C[i][j] = 10;
				else
					C[i][j] = 1 + minNeighborCostToGo(C, i, j, mr, mc);
			}
		}

		if(areEqualArrays(C,Ctemp))
			converged = true;

	}

	cout<<"Here is the cost matrix :"<<endl;
	printMatrix(C);

	int r = startr;
	int c = startc;

	while(!(r == goalr && c == goalc))
	{
		// get min neighbor cost
		int minCost = minNeighborCostToGo(C,r,c, mr, mc);

		cout<<"Got to: "<<mr<<" "<<mc<<endl;

		r = mr;
		c = mc;

	}

	cout<<"End!"<<endl;
	return 0;
}