using System;

using System.Collections.Generic;

using System.Linq;

using System.Text;

using Emgu.CV;

using Emgu.CV.Structure;

using MathNet.Numerics.LinearAlgebra;

using System.Drawing;

using Emgu.CV.Features2D;

using System.Windows.Forms;



namespace SS11P04ImageStitching

{

    class SS11P04IS_Loesung : SS11P04IS_Aufgabe

    {

        /// <summary>

        /// Bitte hier Namen und Matrikelnummern eintragen

        /// </summary>

        public SS11P04IS_Loesung()

        {

            authors = new String[2] { "Sebastian Lichtenfels", "Andreas Mantler" };

            matrikelNummern = new int[2] { 366070, 363220 };

            //Nur der Neugier halber

            OS = OS.Linux;

		}



    	/// <summary>

        /// Zu implementieren: Berechnet eine Gerade y=mx+b zu den Punkten samples mittels RANSAC

        /// </summary>

        /// <param name="samples">Die Datenpunkte (müssen mindestens 2 sein)</param>

        /// <param name="steps">Anzahl der Schritte</param>

        /// <param name="modelThresh">Threshold bis wohin ein Punkt nicht als grober Fehler gilt (der Abstandsbegriff wird durch die Methode distance definiert)</param>

        /// <returns>Eine 1x2-Matrix [m, b]</returns>

        public double[,] ransac(Match[] samples, int steps, double modelThresh)

        {

			Random rng = new Random();

			Matrix A, B, X;



			// Anzahl der Samples:

			int n = samples.Length;



			// Speicherplätze für ConsensusSets:

			// das aktuell verarbeitete; das mit bisher am meisten Elementen

            List<Match> currConsensusSet, biggestConsensusSet = new List<Match>();



			for (int i = 0; i < steps; i++)

            {

				// die zufällig gewählten Punkte holen:

				List<Match> matches = new List<Match>();

				A = new Matrix(3, 3);

				B = new Matrix(3, 3);

				for(int j = 0; j < 3; j++)

				{

					int index;

					do

						index = rng.Next(n);

					while(matches.Contains(samples[index]));

					matches.Add(samples[index]);

					A[j, 0] = samples[index].p1.X;

					A[j, 1] = samples[index].p1.Y;

					A[j, 2] = 1;

					B[j, 0] = samples[index].p2.X;

					B[j, 1] = samples[index].p2.Y;

					B[j, 2] = 1;

				}



	      		//lasse Schritte, in denen A singulär ist, aus, passiert aber sowieso fast nie

	      		try

	      		{

					X = B.Solve(A);

					X.Transpose();

				}

				catch { continue; }



				// Bestimmen des ConsensusSets:

                currConsensusSet = new List<Match>();

                for (int j = 0; j < n; j++)

				{

					// Punkt holen

					Match c = samples[j];

					PointF p1 = new PointF();

					p1.X = (float)(X[0, 0] * c.p2.X + X[0, 1] * c.p2.Y + X[0, 2]);

					p1.Y = (float)(X[1, 0] * c.p2.X + X[1, 1] * c.p2.Y + X[1, 2]);

					float z = (float)(X[2, 0] + X[2, 1] + X[2, 2]);

					p1.X /= z;

					p1.Y /= z;



					double e = Math.Sqrt((c.p1.X - p1.X) * (c.p1.X - p1.X) + (c.p1.Y - p1.Y) * (c.p1.Y - p1.Y));



					// Wenn der Abstand zur Geraden durch p0 und p1 noch im Rahmen liegt, hinzufügen:

                    if (e < modelThresh)

                        currConsensusSet.Add(c);

				}



                // Es wird das ConsensusSet gesucht, das die meisten Elemente enthält:

                if (currConsensusSet.Count > biggestConsensusSet.Count)

                    biggestConsensusSet = currConsensusSet;

			}





			A = new Matrix(biggestConsensusSet.Count, 3);

			B = new Matrix(biggestConsensusSet.Count, 3);

			for(int i = 0; i < biggestConsensusSet.Count; i++)

			{

				A[i, 0] = biggestConsensusSet[i].p1.X;

				A[i, 1] = biggestConsensusSet[i].p1.Y;

				A[i, 2] = 1;

				B[i, 0] = biggestConsensusSet[i].p2.X;

				B[i, 1] = biggestConsensusSet[i].p2.Y;

				B[i, 2] = 1;

			}

            X = B.Solve(A);

			X.Transpose();

			return X.CopyToArray();

		}



        /// <summary>

        /// Findet in den korrespondierenden Punktepaaren correspondences eine Homographie

        /// </summary>

        /// <param name="imageL">Das linke Bild</param>

        /// <param name="imageR">Das rechte Bild</param>

        /// <param name="correspondences">Menge von korrespondierenden Punktepaaren</param>

        /// <returns>Homographiematrix, die die Transformation zwischen linkem und rechtem Bild beschreibt</returns>

        public override double[,] ransacH(

            Image<Bgr, byte> imageL,

            Image<Bgr, byte> imageR,

            Match[] correspondences,

            int ransacSteps,

            double ransacThresh)

        {

			return ransac(correspondences, ransacSteps, ransacThresh);

        }





        /// <summary>

        /// Beinhaltet die eigentliche Transformation. Benutzt zum Transformieren eure Implementierung von

        /// transformPerspectiveBL aus P1. Kommentiert ausführlich, nach welchem Mechanismus ihr

        /// die Bilder überblendet

        /// </summary>

        /// <param name="imageL">Das linke Bild</param>

        /// <param name="imageR">Das rechte Bild</param>

        /// <param name="H">Die Transformation zwischen linkem und rechtem Bild</param>

        /// <returns>Das fertige Panoama</returns>

        public override Image<Bgr, byte> blend(Image<Bgr, byte> imageL, Image<Bgr, byte> imageR, double[,] H)

        {

			Image<Bgr, byte> both = new Image<Bgr, byte>(imageL.Width+imageR.Width, Math.Max(imageL.Height, imageR.Height));





			int rh = (int)(imageL.Width + H[0, 2]);

			H[0, 2] = 0;



			imageR = imageR.WarpPerspective<double>(

                     new Emgu.CV.Matrix<double>(H), //Matrixklasse von Emgu

                     Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR,

                     Emgu.CV.CvEnum.WARP.CV_WARP_DEFAULT,

                     new Bgr(0, 255, 0)

                   ).Convert<Bgr, byte>();



			for(int y = 0; y < imageL.Height; y++)

			{

				for(int x = 0; x < imageL.Width; x++)

				{

					if(imageL.Data[y, x, 0] == 0 && imageL.Data[y, x, 1] == 0 && imageL.Data[y, x, 2] == 0) continue;

					/*float r = 0, g = 0, b = 0;

					if(lh+x > imageL.Width)

					{

						r = (imageL.Data[y, x, 0] - both.Data[y, x+lh, 0]);

						g = (imageL.Data[y, x, 1] - both.Data[y, x+lh, 1]);

						b = (imageL.Data[y, x, 2] - both.Data[y, x+lh, 2]);

						if(r*r + g*g + b*b < 20) break;

						float f = ((float)(lh+x - imageL.Width)) / ((float)lh);

						r *= f;

						g *= f;

						b *= f;

					}*/

					both.Data[y, x, 0] = (byte)(imageL.Data[y, x, 0] - 0);

					both.Data[y, x, 1] = (byte)(imageL.Data[y, x, 1] - 0);

					both.Data[y, x, 2] = (byte)(imageL.Data[y, x, 2] - 0);

				}

			}



			for(int y = 0; y < imageR.Height; y++)

			{

				for(int x = 0; x < imageR.Width; x++)

				{

					if(imageR.Data[y, x, 0] == 0 && imageR.Data[y, x, 1] == 0 && imageR.Data[y, x, 2] == 0) continue;

					both.Data[y, rh+x, 0] = imageR.Data[y, x, 0];

					both.Data[y, rh+x, 1] = imageR.Data[y, x, 1];

					both.Data[y, rh+x, 2] = imageR.Data[y, x, 2];

				}

			}



			return both;

        }



		private const int patchsize = 2;



        /// <summary>

        /// Eure Implementierung der Funktion aus P3. Wird nicht korrigiert.

        /// </summary>

        /// <param name="image"></param>

        /// <param name="a"></param>

        /// <returns></returns>

        public override PointF[] findFeaturePoints(Image<Bgr, byte> image, double a)

        {



            //zu Graubild konvertieren

			Image<Gray, byte> grayimage = image.Convert<Gray, byte>();



			List<PointF> points = new List<PointF>(); //Liste der FeaturePoints

			float[,,] d = new float[grayimage.Height, grayimage.Width, 2]; //partielle Ableitungen (nach x und y) für jeden Pixel. Schema: d[row, column, {x=0 or y=1}]



            //partielle Ableitungen für jeden Pixel bestimmen mit diskretene Werten in einer patchsize-Umgebung um den jeweiligen Pixel

            //patchsize-Umgebung meint hierbei ein Intervall [p.X-patchsize, p.X+patchsize] für die Ableitung nach x und entsprechend [p.Y-patchsize, p.Y+patchsize] für y für alle Pixel p

            //Der Rand des Bildes (Randsträke = patchsize) wird dabei ausgelassen

			for(int r = patchsize; r < grayimage.Height - patchsize; r++)

			{

				for(int c = patchsize; c < grayimage.Width - patchsize; c++)

				{

					float fx = 0, fy = 0;

                    //Alle Pixel in der patchsize-Umgebung

					for(int i = -patchsize; i <= patchsize; i++)

					{

						if(i == 0) continue; //Den Pixel selbst auslassen

						fy += (float)(image.Data[r+i, c, 0] - image.Data[r, c, 0])/(float)i;

						fx += (float)(image.Data[r, c+i, 0] - image.Data[r, c, 0])/(float)i;

					}

                    //Die errechneten Ableitungen abspeichern

					d[r, c, 0] = fx;

					d[r, c, 1] = fy;

				}

			}



			float[,] R = new float[grayimage.Height, grayimage.Width]; //Scalar interest measures (nach Harris) für jeden Pixel

			float mean = 0; //Durchschnitt aller scalar interest measures (SIM)

			for(int r = patchsize; r < grayimage.Height - patchsize; r++)

			{

				for(int c = patchsize; c < grayimage.Width - patchsize; c++)

				{

                    // Die Einträge der Matrix M. M01 = M10, deshab nur einmal gespeichert.

                    //Aus Performance-Gründen wird nicht die Klasse Matrix verwendet

					float M00 = 0, M01 = 0, M11 = 0;

                    //Als Nachbarschaft, wähle eine patchsize*patchsize-Nachbarschaft um den aktuellen Pixel

					for(int nr = -patchsize; nr <= patchsize; nr++)

					{

						for(int nc = -patchsize; nc <= patchsize; nc++)

						{

                            //Berechnung der Matrix-Einträge nach Vorlesung

							float fx = d[r+nr, c+nc, 0];

							float fy = d[r+nr, c+nc, 1];

							M00 += fx*fx;

							M11 += fy*fy;

							M01 += fy*fx;

						}

					}

                    //Berechnung des SIM

					float trace = M00 + M11; //Spur der Matrix M

					R[r, c] = M00*M11 - M01*M01 - (float)a*trace*trace; //= det(M) - a * spur(M)^2

					mean += R[r, c]; //Aufsummieren aller SIM

				}

			}

            //Teilen durch Anzahl der Pixel (dies ist nicht der genaue Durchschnittswert, da der Rand des Bildes nicht berücksichtigt wird, aber praktisch gesehen ein guter Schwellenwert für Feature Points)

            mean /= grayimage.Height * grayimage.Width;



            //Diejenigen Pixel als FeaturePoints deklarieren, die über dem Schwellenwert (=mean) liegen und die ein lokales Maximum in ihrer patchsize*patchsize-Nachbarschaft darstellen

			for(int r = patchsize; r < grayimage.Height - patchsize; r++)

			{

				for(int c = patchsize; c < grayimage.Width - patchsize; c++)

				{

					if(R[r, c] > mean)

					{

                        bool peak = true;

						for(int nr = -patchsize; nr <= patchsize; nr++)

						{

							for(int nc = -patchsize; nc <= patchsize; nc++)

							{

								if(R[r+nr, c+nc] > R[r, c]) //Für alle Pixel der Nachbarschaft prüfen, ob sie größeren SIM haben als der aktuelle Pixel. Ist dies der Fall, so ist der aktuelle Pixel kein lokales Maximum und daher auch kein Feature Point

									peak = false;

							}

						}

                        if (peak)

						    points.Add(new PointF(c, r));

					}

				}

			}

            //Die iste der Feature Points als Array konvertiert zurückgeben

            return points.ToArray();

        }



		// Eine einfache clampfunktion, um Bildkoordinaten an den Rändern an ihre Kanten zu clampen

		private int clamp(int v, int min, int max)

		{

			return Math.Min(max, Math.Max(min, v));

		}



        /// <summary>

        /// Eure Implementierung der Funktion aus P3. Wird nicht korrigiert.

        /// </summary>

        /// <param name="image"></param>

        /// <param name="a"></param>

        /// <returns></returns>

        public override Match[] findCorrespondences(Image<Bgr, byte> refImage, Image<Bgr, byte> tarImage, System.Drawing.PointF[] refFeatures, System.Drawing.PointF[] tarFeatures, int searchRange, int k)

        {



			// Listen für Zwischenergebnisse

			List<Match> matches = new List<Match>();

			List<float> distances = new List<float>();



			// Die Suchentfernung von der Mitte ist nurnoch halb so groß...

			float searchRadius = searchRange/2.0f;



			// Für jeden Referenzpunkt...

			foreach(var refP in refFeatures)

			{

				// Variable für den momentan kleinsten gefundenen SSD-Pixel-"Abstand"

				float minDifference = float.MaxValue;

				// variable für den momentan besten gefundenen Korrespondenten Punkt

				PointF best = tarFeatures[0];



				// Für jeden Targetpunkt

				foreach(var tarP in tarFeatures)

				{

					// Prüfe, ob sich dieser Targetpunkt in der searchRange befindet

					// Hier wird ausgenutzt, dass die Punkte der höhe nach abgespeichert wurden

					if(tarP.Y < refP.Y - searchRadius)

						continue;

					if(tarP.Y > refP.Y + searchRadius)

						break;



					// Der aktuelle SSD-"Abstand" der Nachbarschaft

					float sumDifference = 0;



					// Für die k-Nachbarschaft vom ref- und tar-Punkt:

					for(int c = -k; c <= k; c+=2)

					{

						// momentane Pixelkoordinaten berechnen:

						int rcol = clamp((int)refP.X+c, 0, refImage.Width-1);

						int tcol = clamp((int)tarP.X+c, 0, tarImage.Width-1);

						for(int r = -k; r <= k; r+=2)

						{

							// momentane Pixelkoordinaten berechnen:

							int rrow = clamp((int)refP.Y+r, 0, refImage.Height-1);

							int trow = clamp((int)tarP.Y+r, 0, tarImage.Height-1);



							// beide Pixel holen:

							byte rb = refImage.Data[rrow, rcol, 0];

							byte rg = refImage.Data[rrow, rcol, 1];

							byte rr = refImage.Data[rrow, rcol, 2];



							byte tb = tarImage.Data[trow, tcol, 0];

							byte tg = tarImage.Data[trow, tcol, 1];

							byte tr = tarImage.Data[trow, tcol, 2];



							// SSD-"Abstand" der aktuellen Pixel berechnen:

							sumDifference += (tb-rb)*(tb-rb) + (tg-rg)*(tg-rg) + (tr-rr)*(tr-rr);

						}

					}



					// Ist dieser Abstand kleiner/besser?

					if(sumDifference < minDifference)

					{

						// Die besten Ergebnisse aktualisieren:

						minDifference = sumDifference;

						best = tarP;

					}

				}



				// Ist dieser Abstand besser als ein Threshold (, der von k abhängt)?

				if(minDifference < k*k*1024.0f)

				{

					// Dann diesen Match zu der Ergebnisliste hinzufügen

					var m = new Match(refP, best);

					matches.Add(m);

					// Und einen Abstand zwischen den Punkten berechnen, um nachher Ausreißer rauszunehmen

					distances.Add((m.p1.X-m.p2.X) * (m.p1.X-m.p2.X) + (m.p1.Y-m.p2.Y) * (m.p1.Y-m.p2.Y));

				}

			}



			// Wir haben keine potenziellen Korrespondenzen gefunden

			if(distances.Count == 0)

				return new Match[0];



			// Medianberechnung

			List<float> sortedDistances = new List<float>(distances);

			sortedDistances.Sort();

			float distanceMedian = sortedDistances[distances.Count/2];



			// Matches, die eine Entfernung haben, die um den Median liegt,

			// (in Abhängigkeit von searchRange) zusammensuchen

			List<Match> finalmatches = new List<Match>();

			for(int i = 0; i < matches.Count; i++)

				if(Math.Abs(distances[i]-distanceMedian) < searchRange * 64.0f)

					finalmatches.Add(matches[i]);



			// Endgültige Funde zurückgeben

			return finalmatches.ToArray();

        }

    }

}