using System;


namespace EearthMoverDistance
{
	public class EMD : IDisposable
	{
		#region IDisposable Members
		public EMD()
		{

		}
		~EMD()
		{
			Dispose();
		}
		public void Dispose()
		{
		}
		#endregion

		public class Feature
		{
			public Feature(double[] f_)
			{
				size = f_.Length;
				f = f_;
			}
			public int size = 0;
			public double[] f = null;
		}
		public class Signature
		{
			public Signature(Feature[] features, double[] w)
			{
				this.n = features.Length;
				this.features = features;
				this.weights = w;
			}
			public int n;						// Number of features in the signature 
			public Feature[] features = null;	// Pointer target the features vector 
			public double[] weights = null;		// Pointer target the weights of the features 
		}
		public class Flow
		{
			public int source = -1;				// Feature number in signature 1 
			public int target = -1;				// Feature number in signature 2 
			public double amount = 0;			// Amount of flow source "source" target "target" 
		}

		// NEW TYPES DEFINITION 
		// SingleLinkedNode IS USED FOR SINGLE-LINKED LISTS 
		private class SingleLinkedNode
		{
			public int i = -1;
			public double val = 0;
			public SingleLinkedNode next = null;
		}
		private class DoubleLinkedNode
		{
			public int i = -1;
			public int j = -1;
			public double val = 0;
			public DoubleLinkedNode nextc = null;		// NEXT COLUMN 
			public DoubleLinkedNode nextr = null;		// NEXT ROW 
		}

		// INTERNAL VARIABLE DECLARATION 
		private static int _n1 = 0;					// SIGNATURES SIZES 
		private static int _n2 = 0;
		private double[][] _C = null;		
		private DoubleLinkedNode[] _X = null;		// THE BASIC VARIABLES VECTOR 
		// VARIABLES TO HANDLE _X EFFICIENTLY 
		private static DoubleLinkedNode _EndX = null;
		private static DoubleLinkedNode _EnterX = null;
		private bool[][] _IsX = null;
		private DoubleLinkedNode[] _RowsX = null;
		private DoubleLinkedNode[] _ColsX = null;
		private static double _maxW = 0;
		private static double _maxC = 0;

		private const int maxSigSize = 100;
		private const int maxIteration = 500;
		private const double infinity = 1e20;
		private const double epsilon = 1e-6;
		private const int maxSigSize1 = (maxSigSize + 1);
		private static int endIndex = 0;

		private void printSolution()
		{
			DoubleLinkedNode P;
			double totalCost;

			totalCost = 0;

			Console.WriteLine("SIG1\tSIG2\tFLOW\tCOST\n");
			for (int i = 0; i < endIndex; ++i)
			{
				P = _X[i];
				if (P != _EnterX && _IsX[P.i][P.j])
				{
					Console.WriteLine(P.i.ToString() + "       " + P.j.ToString() +
						"       " + P.val.ToString() + "      " + _C[P.i][P.j].ToString() + "\n");
					totalCost += (double)P.val * _C[P.i][P.j];
				}
			}
			Console.WriteLine("COST = " + totalCost.ToString() + "\n");
		}
		public double Solve(Signature s1, Signature s2, Flow[] flow, ref int flowSize)
		{
			int itr;
			double totalCost;
			double w;
			DoubleLinkedNode XP = null;
			Flow FlowP = null;

			SingleLinkedNode[] U = new SingleLinkedNode[maxSigSize1];
			SingleLinkedNode[] V = new SingleLinkedNode[maxSigSize1];
			for (itr = 0; itr < maxSigSize1; ++itr)
			{
				U[itr] = new SingleLinkedNode();
				V[itr] = new SingleLinkedNode();
			}
			this._X = new DoubleLinkedNode[maxSigSize1 * 2];
			for (itr = 0; itr < maxSigSize1 * 2; ++itr)
			{
				this._X[itr] = new DoubleLinkedNode();
			}

			w = Init(s1, s2);

			Console.WriteLine("\nINITIAL SOLUTION:\n");
			printSolution();

			if (_n1 > 1 && _n2 > 1) // IF _n1 = 1 OR _n2 = 1 THEN WE ARE DONE 
			{
				for (itr = 1; itr < maxIteration; itr++)
				{
					// FIND BASIC VARIABLES 
					FindBasicVariables(U, V);

					// CHECK FOR OPTIMALITY 
					if (IsOptimal(U, V))
						break;

					// IMPROVE SOLUTION 
					NewSol();

					Console.WriteLine("\nITERATION # " + itr.ToString() + "\n");
					printSolution();

				}

				if (itr == maxIteration)
					Console.WriteLine("emd: Maximum number of iterations has been reached ()\n");
			}

			// COMPUTE THE TOTAL FLOW 
			totalCost = 0;
			if (flow != null)
				FlowP = flow[0];
			int count = 0;
			for (int i = 0; i < _X.Length; i++)
			{
				XP = _X[i];
				if (XP.Equals(_EndX)) break;

				if (XP == _EnterX) // _EnterX IS THE EMPTY SLOT 
					continue;
				if (XP.i == s1.n || XP.j == s2.n) // DUMMY FEATURE 
					continue;

				if (XP.val == 0) // ZERO FLOW 
					continue;

				totalCost += (double)XP.val * _C[XP.i][XP.j];
				if (flow != null)
				{
					FlowP.source = XP.i;
					FlowP.target = XP.j;
					FlowP.amount = (double)XP.val;
					FlowP = flow[++count];
				}
			}
			if (flow != null)
				flowSize = count;


			Console.WriteLine("\n*** OPTIMAL SOLUTION (" + itr.ToString() +" ITERATIONS): " + 
			totalCost.ToString() + " ***\n");

			// RETURN THE NORMALIZED COST == EMD 
			return (double)(totalCost / w);
		}
		// subfunction called.
		private double Dist(Feature f1, Feature f2)
		{
			if (f1.size != f2.size)
			{
				throw new Exception();
			}
			double[] df = new double[f1.size]; double sum = 0;
			for (int i = 0; i < f1.size; ++i)
			{
				df[i] = f1.f[i] - f2.f[i];
				sum += df[i] * df[i];
			}
			return Math.Sqrt(sum);
		}
		private double Init(Signature s1, Signature s2)
		{
			int i;
			int j;
			double sSum;
			double dSum;
			double diff;
			Feature P1;
			Feature P2;
			double[] S = new double[maxSigSize1];
			double[] D = new double[maxSigSize1];

			_n1 = s1.n;
			_n2 = s2.n;

			//initialize all the variables, youyi
			this._RowsX = new DoubleLinkedNode[maxSigSize + 1];
			this._ColsX = new DoubleLinkedNode[maxSigSize + 1];
			this._IsX = new bool[maxSigSize + 1][];
			for (i = 0; i < maxSigSize + 1; ++i)
				this._IsX[i] = new bool[maxSigSize + 1];
			this._C = new double[_n1][];
			for (i = 0; i < _n1; ++i)
				this._C[i] = new double[_n2];

			if (_n1 > maxSigSize || _n2 > maxSigSize)
			{
				throw new ArgumentException();
			}

			// COMPUTE THE DISTANCE MATRIX 
			_maxC = 0;
			for (i = 0; i < _n1; i++)
			{
				P1 = s1.features[i];
				for (j = 0; j < _n2; j++)
				{
					P2 = s2.features[j];
					this._C[i][j] = Dist(P1, P2);
					if (this._C[i][j] > _maxC)
						_maxC = _C[i][j];
				}
			}

			// SUM UP THE SUPPLY AND DEMAND 
			sSum = 0.0;
			for (i = 0; i < _n1; i++)
			{
				S[i] = s1.weights[i];
				sSum += s1.weights[i];
				_RowsX[i] = null;
			}
			dSum = 0.0;
			for (j = 0; j < _n2; j++)
			{
				D[j] = s2.weights[j];
				dSum += s2.weights[j];
				_ColsX[j] = null;
			}

			// IF SUPPLY DIFFERENT THAN THE DEMAND, ADD A ZERO-COST DUMMY CLUSTER 
			diff = sSum - dSum;
			if (Math.Abs(diff) >= epsilon * sSum)
			{
				if (diff < 0.0)
				{
					for (j = 0; j < _n2; j++)
						_C[_n1][j] = 0;
					S[_n1] = -diff;
					_RowsX[_n1] = null;
					_n1++;
				}
				else
				{
					for (i = 0; i < _n1; i++)
						_C[i][_n2] = 0;
					D[_n2] = diff;
					_ColsX[_n2] = null;
					_n2++;
				}
			}

			// INITIALIZE THE BASIC VARIABLE STRUCTURES 
			for (i = 0; i < _n1; i++)
				for (j = 0; j < _n2; j++)
					_IsX[i][j] = false;
			_EndX = _X[0];

			_maxW = sSum > dSum ? sSum : dSum;

			// FIND INITIAL SOLUTION 
			Russel(S, D);

			_EnterX = _EndX;
			_EndX = _X[++endIndex]; // AN EMPTY SLOT (ONLY _n1+_n2-1 BASIC VARIABLES) 

			return sSum > dSum ? dSum : sSum;
		}
		
		private bool IsOptimal(SingleLinkedNode[] U, SingleLinkedNode[] V)
		{
			double delta;
			double deltaMin;
			int i;
			int j;
			int minI = int.MaxValue;
			int minJ = int.MaxValue;

			// FIND THE MINIMAL Cij-Ui-Vj OVER ALL i,j 
			deltaMin = infinity;
			for (i = 0; i < _n1; i++)
				for (j = 0; j < _n2; j++)
					if (!this._IsX[i][j])
					{
						delta = _C[i][j] - U[i].val - V[j].val;
						if (deltaMin > delta)
						{
							deltaMin = delta;
							minI = i;
							minJ = j;
						}
					}

			if (deltaMin == infinity)
			{
				throw new Exception();
			}

			_EnterX.i = minI;
			_EnterX.j = minJ;

			// IF NO NEGATIVE deltaMin, WE FOUND THE OPTIMAL SOLUTION 
			return deltaMin >= -epsilon * _maxC;

			//
			//   return deltaMin >= -epsilon;
			// 
		}
		private void NewSol()
		{
			int i;
			int j;
			int k;
			double xMin;
			int steps;
			DoubleLinkedNode[] Loop = new DoubleLinkedNode[2 * maxSigSize1];
			DoubleLinkedNode CurX = null;
			DoubleLinkedNode LeaveX = null;

			// ENTER THE NEW BASIC VARIABLE 
			i = _EnterX.i;
			j = _EnterX.j;
			_IsX[i][j] = true;
			_EnterX.nextc = _RowsX[i];
			_EnterX.nextr = _ColsX[j];
			_EnterX.val = 0;
			_RowsX[i] = _EnterX;
			_ColsX[j] = _EnterX;

			// FIND A CHAIN REACTION 
			steps = FindLoop(Loop);

			// FIND THE LARGEST VALUE IN THE LOOP 
			xMin = infinity;
			for (k = 1; k < steps; k += 2)
			{
				if (Loop[k].val < xMin)
				{
					LeaveX = Loop[k];
					xMin = Loop[k].val;
				}
			}

			// UPDATE THE LOOP 
			for (k = 0; k < steps; k += 2)
			{
				Loop[k].val += xMin;
				Loop[k + 1].val -= xMin;
			}

			// REMOVE THE LEAVING BASIC VARIABLE 
			i = LeaveX.i;
			j = LeaveX.j;
			_IsX[i][j] = false;
			if (_RowsX[i] == LeaveX)
				_RowsX[i] = LeaveX.nextc;
			else
				for (CurX = _RowsX[i]; CurX != null; CurX = CurX.nextc)
					if (CurX.nextc == LeaveX)
					{
						CurX.nextc = CurX.nextc.nextc;
						break;
					}
			if (_ColsX[j] == LeaveX)
				_ColsX[j] = LeaveX.nextr;
			else
				for (CurX = _ColsX[j]; CurX != null; CurX = CurX.nextr)
					if (CurX.nextr == LeaveX)
					{
						CurX.nextr = CurX.nextr.nextr;
						break;
					}

			// SET _EnterX TO BE THE NEW EMPTY SLOT 
			_EnterX = LeaveX;
		}
		private int IndexOf(DoubleLinkedNode t, DoubleLinkedNode[] x)
		{
			for (int i = 0; i < x.Length; ++i)
			{
				if (x[i].Equals(t))
					return i;
			}
			return -1;
		}
		private int FindLoop(DoubleLinkedNode[] Loop)
		{
			int i, steps;
			DoubleLinkedNode[] CurX = null;
			DoubleLinkedNode NewX = null;

			bool[] IsUsed = new bool[2 * maxSigSize1];
			for (i = 0; i < _n1 + _n2; i++)
				IsUsed[i] = false;

			CurX = Loop;
			NewX = CurX[0] = _EnterX;
			for (i = 0; i < _X.Length; ++i)
			{
				if (_EnterX.Equals(_X[i]))
					break;
			}
			IsUsed[i] = true;
			steps = 1;
			int iIndex = 0;
			do
			{
				if (steps % 2 == 1)
				{
					// FIND AN UNUSED X IN THE ROW 
					NewX = _RowsX[NewX.i];
					while (NewX != null && IsUsed[IndexOf(NewX, _X)])
						NewX = NewX.nextc;
				}
				else
				{
					// FIND AN UNUSED X IN THE COLUMN, OR THE ENTERING X 
					NewX = _ColsX[NewX.j];
					while (NewX != null && IsUsed[IndexOf(NewX, _X)] && NewX != _EnterX)
						NewX = NewX.nextr;
					if (NewX == _EnterX)
						break;
				}

				if (NewX != null) // FOUND THE NEXT X 
				{
					// ADD X TO THE LOOP 
					CurX[++iIndex] = NewX;
					IsUsed[IndexOf(NewX, _X)] = true;
					steps++;
				}
				else // DIDN'T FIND THE NEXT X 
				{
					// BACKTRACK 
					do
					{
						NewX = CurX[iIndex];
						do
						{
							if (steps % 2 == 1)
								NewX = NewX.nextr;
							else
								NewX = NewX.nextc;
						} while (NewX != null && IsUsed[IndexOf(NewX, _X)]);

						if (NewX == null)
						{
							IsUsed[IndexOf(CurX[iIndex], _X)] = false;
							iIndex--;
							steps--;
						}
					} while (NewX == null && iIndex >= 0);

					IsUsed[IndexOf(CurX[iIndex], _X)] = true;
					CurX[iIndex] = NewX;
					IsUsed[IndexOf(NewX, _X)] = true;
				}
			} while (iIndex >= 0);

			if (CurX[iIndex].Equals(Loop[0]))
			{
				throw new Exception();
			}

			return steps;
		}
		private void Russel(double[] S, double[] D)
		{
			int i;
			int j;
			bool found;
			int minI = -1;
			int minJ = -1;
			double deltaMin;
			double oldVal;
			double diff;
			double[,] Delta = new double[maxSigSize1, maxSigSize1];
			SingleLinkedNode[] Ur = new SingleLinkedNode[maxSigSize1];
			SingleLinkedNode[] Vr = new SingleLinkedNode[maxSigSize1];
			for (i = 0; i < maxSigSize1; ++i)
			{
				Ur[i] = new SingleLinkedNode();
				Vr[i] = new SingleLinkedNode();
			}
			SingleLinkedNode uHead = new SingleLinkedNode();
			SingleLinkedNode CurU = null;
			SingleLinkedNode PrevU = null;
			SingleLinkedNode vHead = new SingleLinkedNode();
			SingleLinkedNode CurV = null;
			SingleLinkedNode PrevV = null;
			SingleLinkedNode PrevUMinI = null;
			SingleLinkedNode PrevVMinJ = null;
			SingleLinkedNode Remember = null;

			// INITIALIZE THE ROWS LIST (Ur), AND THE COLUMNS LIST (Vr) 
			uHead.next = CurU = Ur[0];
			for (i = 0; i < _n1; i++)
			{
				CurU.i = i;
				CurU.val = -infinity;
				CurU.next = Ur[i + 1];
				CurU = Ur[i + 1];
			}
			Ur[_n1-1].next = null;

			vHead.next = CurV = Vr[0];
			for (j = 0; j < _n2; j++)
			{
				CurV.i = j;
				CurV.val = -infinity;
				CurV.next = Vr[j + 1];
				CurV = Vr[j + 1];
			}
			Vr[_n2-1].next = null;

			// FIND THE MAXIMUM ROW AND COLUMN VALUES (Ur[i] AND Vr[j]) 
			for (i = 0; i < _n1; i++)
				for (j = 0; j < _n2; j++)
				{
					double v;
					v = _C[i][j];
					if (Ur[i].val <= v)
						Ur[i].val = v;
					if (Vr[j].val <= v)
						Vr[j].val = v;
				}

			// COMPUTE THE Delta MATRIX 
			for (i = 0; i < _n1; i++)
				for (j = 0; j < _n2; j++)
					Delta[i, j] = _C[i][j] - Ur[i].val - Vr[j].val;

			do
			{
				// FIND THE SMALLEST Delta[i][j] 
				found = false;
				deltaMin = infinity;
				PrevU = uHead;
				for (CurU = uHead.next; CurU != null; CurU = CurU.next)
				{
					i = CurU.i;
					PrevV = vHead;
					for (CurV = vHead.next; CurV != null; CurV = CurV.next)
					{
						j = CurV.i;
						if (deltaMin > Delta[i, j])
						{
							deltaMin = Delta[i, j];
							minI = i;
							minJ = j;
							PrevUMinI = PrevU;
							PrevVMinJ = PrevV;
							found = true;
						}
						PrevV = CurV;
					}
					PrevU = CurU;
				}

				if (!found)
					break;

				// ADD X[minI][minJ] TO THE BASIS, AND ADJUST SUPPLIES AND COST 
				Remember = PrevUMinI.next;
				AddBasicVariable(minI, minJ, ref S, ref D, ref PrevUMinI, ref PrevVMinJ, uHead);

				// UPDATE THE NECESSARY Delta[][] 
				if (Remember == PrevUMinI.next) // LINE minI WAS DELETED 
				{
					for (CurV = vHead.next; CurV != null; CurV = CurV.next)
					{
						j = CurV.i;
						if (CurV.val == _C[minI][j]) // COLUMN j NEEDS UPDATING 
						{
							// FIND THE NEW MAXIMUM VALUE IN THE COLUMN 
							oldVal = CurV.val;
							CurV.val = -infinity;
							for (CurU = uHead.next; CurU != null; CurU = CurU.next)
							{
								i = CurU.i;
								if (CurV.val <= _C[i][j])
									CurV.val = _C[i][j];
							}

							// IF NEEDED, ADJUST THE RELEVANT Delta[*][j] 
							diff = oldVal - CurV.val;
							if (Math.Abs(diff) < epsilon * _maxC)
								for (CurU = uHead.next; CurU != null; CurU = CurU.next)
									Delta[CurU.i, j] += diff;
						}
					}
				}
				else // COLUMN minJ WAS DELETED 
				{
					for (CurU = uHead.next; CurU != null; CurU = CurU.next)
					{
						i = CurU.i;
						if (CurU.val == _C[i][minJ]) // ROW i NEEDS UPDATING 
						{
							// FIND THE NEW MAXIMUM VALUE IN THE ROW 
							oldVal = CurU.val;
							CurU.val = -infinity;
							for (CurV = vHead.next; CurV != null; CurV = CurV.next)
							{
								j = CurV.i;
								if (CurU.val <= _C[i][j])
									CurU.val = _C[i][j];
							}

							// If NEEDED, ADJUST THE RELEVANT Delta[i][*] 
							diff = oldVal - CurU.val;
							if (Math.Abs(diff) < epsilon * _maxC)
								for (CurV = vHead.next; CurV != null; CurV = CurV.next)
									Delta[i, CurV.i] += diff;
						}
					}
				}
			} while (uHead.next != null || vHead.next != null);
		}
		private void FindBasicVariables(SingleLinkedNode[] U, SingleLinkedNode[] V)
		{
			int i;
			int j;
			int UfoundNum;
			int VfoundNum;
			SingleLinkedNode u0Head = new SingleLinkedNode();
			SingleLinkedNode u1Head = new SingleLinkedNode();
			SingleLinkedNode v0Head = new SingleLinkedNode();
			SingleLinkedNode v1Head = new SingleLinkedNode();
			SingleLinkedNode CurU = null, PrevU = null;
			SingleLinkedNode CurV = null, PrevV = null;

			// INITIALIZE THE ROWS LIST (U) AND THE COLUMNS LIST (V) 
			u0Head.next = CurU = U[0];
			for (i = 0; i < _n1; i++)
			{
				CurU.i = i;
				CurU.next = U[i + 1];
				CurU = U[i + 1];
			}
			U[_n1 - 1].next = null;
			u1Head.next = null;

			CurV = V[1];
			v0Head.next = _n2 > 1 ? V[1] : null;
			for (j = 1; j < _n2; j++)
			{
				CurV.i = j;
				CurV.next = V[j + 1];
				CurV = V[j + 1];
			}
			V[_n2 - 1].next = null;
			v1Head.next = null;

			//   THERE ARE _n1+_n2 VARIABLES BUT ONLY _n1+_n2-1 INDEPENDENT EQUATIONS,
			//     SO SET V[0]=0 
			V[0].i = 0;
			V[0].val = 0;
			v1Head.next = V[0];
			v1Head.next.next = null;

			// LOOP UNTIL ALL VARIABLES ARE FOUND 
			UfoundNum = VfoundNum = 0;
			while (UfoundNum < _n1 || VfoundNum < _n2)
			{
				if (VfoundNum < _n2)
				{
					// LOOP OVER ALL MARKED COLUMNS 
					PrevV = v1Head;
					for (CurV = v1Head.next; CurV != null; CurV = CurV.next)
					{
						j = CurV.i;
						// FIND THE VARIABLES IN COLUMN j 
						PrevU = u0Head;
						for (CurU = u0Head.next; CurU != null; CurU = CurU.next)
						{
							i = CurU.i;
							if (this._IsX[i][j])
							{
								// COMPUTE U[i] 
								CurU.val = _C[i][j] - CurV.val;
								// ...AND ADD IT TO THE MARKED LIST 
								PrevU.next = CurU.next;
								CurU.next = u1Head.next != null ? u1Head.next : null;
								u1Head.next = CurU;
								CurU = PrevU;
							}
							else
								PrevU = CurU;
						}
						PrevV.next = CurV.next;
						VfoundNum++;
					}
				}
				if (UfoundNum < _n1)
				{
					// LOOP OVER ALL MARKED ROWS 
					PrevU = u1Head;
					for (CurU = u1Head.next; CurU != null; CurU = CurU.next)
					{
						i = CurU.i;
						// FIND THE VARIABLES IN ROWS i 
						PrevV = v0Head;
						for (CurV = v0Head.next; CurV != null; CurV = CurV.next)
						{
							j = CurV.i;
							if (_IsX[i][j])
							{
								// COMPUTE V[j] 
								CurV.val = _C[i][j] - CurU.val;
								// ...AND ADD IT TO THE MARKED LIST 
								PrevV.next = CurV.next;
								CurV.next = v1Head.next != null ? v1Head.next : null;
								v1Head.next = CurV;
								CurV = PrevV;
							}
							else
								PrevV = CurV;
						}
						PrevU.next = CurU.next;
						UfoundNum++;
					}
				}
			}
		}
		private void AddBasicVariable(int minI, int minJ, ref double[] S, ref double[] D,
			ref SingleLinkedNode PrevUMinI, ref SingleLinkedNode PrevVMinJ, SingleLinkedNode UHead)
		{
			double T;

			if (Math.Abs(S[minI] - D[minJ]) <= epsilon * _maxW) // DEGENERATE CASE 
			{
				T = S[minI];
				S[minI] = 0;
				D[minJ] -= T;
			}
			else if (S[minI] < D[minJ]) // SUPPLY EXHAUSTED 
			{
				T = S[minI];
				S[minI] = 0;
				D[minJ] -= T;
			}
			else // DEMAND EXHAUSTED 
			{
				T = D[minJ];
				D[minJ] = 0;
				S[minI] -= T;
			}

			// X(minI,minJ) IS A BASIC VARIABLE 
			_IsX[minI][minJ] = true;

			_EndX.val = T;
			_EndX.i = minI;
			_EndX.j = minJ;
			_EndX.nextc = _RowsX[minI];
			_EndX.nextr = _ColsX[minJ];
			_RowsX[minI] = _EndX;
			_ColsX[minJ] = _EndX;
			_EndX = _X[++endIndex];

			// DELETE SUPPLY ROW ONLY IF THE EMPTY, AND IF NOT LAST ROW 
			if (S[minI] == 0 && UHead.next.next != null)
				PrevUMinI.next = PrevUMinI.next.next; // REMOVE ROW FROM LIST 
			else
				PrevVMinJ.next = PrevVMinJ.next.next; // REMOVE COLUMN FROM LIST 
		}
	}
}
