//PlaneCalibration class...
/// <summary>
/// The plane callibration class handles the transformation that map points from the raster space of
/// the video feed to NDC coordinates. It accomplishes this transformation by analying an image from
/// the video feed to identify and store the corner locations of the screen in the image. A homography is
/// found that maps these points to NDC corners. 
/// </summary>
public class PlaneCalibration : CallibrationData
{
	/*
	 * CONSTANTS
	 */
	public const int GREEN_CORNER_THRESHOLD = 10;
	public const int BOUNDS_PADDING = 10;

	/*
	 * CLASS PROPERTIES
	 */
	private bool HomFound;
	private Matrix3 HomMatrix;

	private List<PictureBox> hCircles;

	private Vector2 cornerTL;
	private Vector2 cornerTR;
	private Vector2 cornerBL;
	private Vector2 cornerBR;

	private WinDraw.Rectangle bounds;


	/*
	 * CONSTRUCTOR
	 */
	/// <summary>
	/// constructor for a new plane callibration object
	/// </summary>
	/// <param name="newCaptureDeviceName">the name of the capture device</param>
	/// <param name="videoResolution">the resolution of the video feed</param>
	/// <param name="newMonitor">the form object for displaying the video feed</param>
	/// <param name="newCaptureDevice">the new capture device object</param>
	/// <param name="newParentForm">the calibration form object</param>
	public PlaneCalibration(String newCaptureDeviceName,
		WinDraw.Size videoResolution, PictureBox newMonitor,
		CaptureDevice newCaptureDevice, CalibrationForm newCalibrationForm)
		: base( newName, videoResolution, newMonitor, newDevice, newParentForm)
	{
		HomFound = false;
		HomMatrix = Matrix3.Identity;
		hCircles = new List<PictureBox>();
	}

	//manual point manipualtion omitted

	/*
	 * AUTO CALIBRATION
	 */
	/// <summary>
	/// Automatically identifies the corners of the screen
	/// in the video feed image (using the most recent frame).
	/// The application displays an image with green circles in the four
	/// corners of the screen so that only a few pixels need to be considered.
	/// </summary>
	public override void AutoUpdateCorners()
	{
		//verify non null values
		if (displayFrame == null || videoMonitor == null)
			throw new Exception("null component, cannot proceed with auto calibration");

		//get most recent frame as a bitmap in unmanaged memory
		Bitmap cornerRefBitmap = (Bitmap)displayFrame.Clone();
		UnsafeBitmap cornerRef = new UnsafeBitmap(cornerRefBitmap);
		cornerRef.LockBitmap();

		//brightest pixels
		List<Vector2d> brightPoints = new List<Vector2d>();

		// identify all points bright enough to
		// be considered part of the screen image
		for (int y = 0; y < cornerRef.Bitmap.Height; y++)
		{
			for (int x = 0; x < cornerRef.Bitmap.Width; x++)
			{
				//for each pixel in image checkt to see if pixel is bright enough
				//to be a corner area pixel
				if (cornerRef.GetPixel(x, y).green > GREEN_CORNER_THRESHOLD)
					brightPoints.Add(new Vector2d(x, y));
			}
		}

		if (brightPoints.Count < 4)
			throw new Exception("Not enough points found.");

		//find the hull of the bright pixels
		double minX = brightPoints[0].X;
		double maxX = brightPoints[0].X;
		double minY = brightPoints[0].Y;
		double maxY = brightPoints[0].Y;

		for (int i = 1; i < brightPoints.Count; i++)
		{
			if (brightPoints[i].X < minX)
				minX = brightPoints[i].X;
			if (brightPoints[i].Y < minY)
				minY = brightPoints[i].Y;
			if (brightPoints[i].X > maxX)
				maxX = brightPoints[i].X;
			if (brightPoints[i].Y > maxY)
				maxY = brightPoints[i].Y;
		}

		//boudning box of the corner points
		bounds = new System.Drawing.Rectangle((int)minX, (int)minY, (int)(maxX - minX), (int)(maxY - minY));

		//points on the corners of the bounding box
		Vector2d boundsTL = new Vector2d(minX, minY);
		Vector2d boundsTR = new Vector2d(maxX, minY);
		Vector2d boundsBL = new Vector2d(minX, maxY);
		Vector2d boundsBR = new Vector2d(maxX, maxY);

		//direction of the bounding box edges
		Vector2d topDir = boundsTR - boundsTL;
		Vector2d bottomDir = boundsBR - boundsBL;
		Vector2d leftDir = boundsTL - boundsBL;
		Vector2d rightDir = boundsTR - boundsBR;

		//screen corners
		Vector2d TL = Vector2d.Zero;
		Vector2d TR = Vector2d.Zero;
		Vector2d BL = Vector2d.Zero;
		Vector2d BR = Vector2d.Zero;

		//edge lengths
		double topLength = Vector2d.DistanceSquared(boundsTL, boundsTR);
		double bottomLength = Vector2d.DistanceSquared(boundsBL, boundsBR);
		double rightLength = Vector2d.DistanceSquared(boundsTR, boundsBR);
		double leftLength = Vector2d.DistanceSquared(boundsTL, boundsBL);

		double curDist = 0;
		double TLMinDist = 0;
		double TRMinDist = 0;
		double BLMinDist = 0;
		double BRMinDist = 0;

		// find the positions that minimizes the distance between
		// two edges of the bounding box to find the screen corner
		for (int i = 0; i < brightPoints.Count; i++)
		{
			//top and left
			curDist = distanceToLine(boundsTL, topDir, topLength, brightPoints[i]) + distanceToLine(boundsBL, leftDir, leftLength, brightPoints[i]);
			if (curDist < TLMinDist || i == 0)
			{
				TL = brightPoints[i];
				TLMinDist = curDist;
			}

			//top and right
			curDist = distanceToLine(boundsTL, topDir, topLength, brightPoints[i]) + distanceToLine(boundsBR, rightDir, rightLength, brightPoints[i]);
			if (curDist < TRMinDist || i == 0)
			{
				TR = brightPoints[i];
				TRMinDist = curDist;
			}

			//bottom and left
			curDist = distanceToLine(boundsBL, bottomDir, bottomLength, brightPoints[i]) + distanceToLine(boundsBL, leftDir, leftLength, brightPoints[i]);
			if (curDist < BLMinDist || i == 0)
			{
				BL = brightPoints[i];
				BLMinDist = curDist;
			}

			//bottom and right
			curDist = distanceToLine(boundsBL, bottomDir, bottomLength, brightPoints[i]) + distanceToLine(boundsBR, rightDir, rightLength, brightPoints[i]);
			if (curDist < BRMinDist || i == 0)
			{
				BR = brightPoints[i];
				BRMinDist = curDist;
			}
		}

		//convert to XNA vectors
		cornerTL = TL.ToXNAVector();
		cornerTR = TR.ToXNAVector();
		cornerBL = BL.ToXNAVector();
		cornerBR = BR.ToXNAVector();

		//output image with identified corners circled for testing purposes
		cornerRef.UnlockBitmap();
		Graphics dc = Graphics.FromImage(cornerRefBitmap);
		Pen p = new Pen(Color.Red, 1);
		dc.DrawEllipse(p, (float)cornerTL.X - 2, (float)cornerTL.Y - 2, 4, 4);
		dc.DrawEllipse(p, (float)cornerTR.X - 2, (float)cornerTR.Y - 2, 4, 4);
		dc.DrawEllipse(p, (float)cornerBL.X - 2, (float)cornerBL.Y - 2, 4, 4);
		dc.DrawEllipse(p, (float)cornerBR.X - 2, (float)cornerBR.Y - 2, 4, 4);
		dc.Dispose();

		cornerRefBitmap.Save(curGame.ApplicationDirectory + CallibrationData.CORNER_REF_IMAGE, WinImage.ImageFormat.Jpeg);

		//update corner point positions (hCircles list)
		//in video monitor on calibration form
		updateCorners();
	}

	/// <summary>
	/// crop video feed to the corners of the screen image area
	/// </summary>
	public override void AutoCrop()
	{
		base.autoCrop();

		if (bounds != null && bounds.Width > 0 && bounds.Height > 0)
		{
			//add padding to bounding box
			bounds.X -= BOUNDS_PADDING;
			bounds.Y -= BOUNDS_PADDING;
			float twicePadding = BOUNDS_PADDING * 2;
			bounds.Width += twicePadding;
			bounds.Height += twicePadding;

			if (bounds.X < 0)
				bounds.X = 0;
			if (bounds.Y < 0)
				bounds.Y = 0;
			if (bounds.Right > fullResolution.Width - 1)
				bounds.Width = fullResolution.Width - bounds.X - 1;
			if (bounds.Bottom > fullResolution.Height - 1)
				bounds.Height = fullResolution.Height - bounds.Y - 1;

			//update crop...
			this.cropRectangle = bounds;
		}
	}

	/*
	 * HOMOGRAPHIC MATRIX CALCULATION
	 */
	/// <summary>
	/// put point positions into a list in the correct
	/// order for homography calculation(TL, TR, BL, BR)
	/// </summary>
	private void getCornerPoints()
	{
		if (hCircles.Count >= 4)
		{
			List<Vector2d> screenCorners = new List<Vector2d>();
			for (int i = 0; i < 4; i++)
			{
				//correct for video scaling and cropping
				screenCorners.Add(new Vector2d(hCircles[i].Location));
				if (mKfound)
					screenCorners[i] = tNormLensInv.transform(radialDistort(tNormLens.transform(screenCorners[i]), Kvector));
			}

			//identify each of 4 corners as the top left, top right, bottom left, and bottom right positions
			double minX = fullResolution.Width;
			double minX2 = fullResolution.Width;
			double minY = fullResolution.Height;
			double minY2 = fullResolution.Height;
			double minXIndex = 0;
			double minX2Index = 0;
			double minYIndex = 0;
			double minY2Index = 0;

			//find smallest two X coordinates
			for (int i = 0; i < 4; i++)
			{
				if (screenCorners[i].X < minX)
				{
					minX2 = minX;
					minX2Index = minXIndex;

					minX = screenCorners[i].X;
					minXIndex = i;
				}
				else if (screenCorners[i].X < minX2)
				{
					minX2 = screenCorners[i].X;
					minX2Index = i;
				}
			}

			//find smallest two Y coordinates
			for (int i = 0; i < 4; i++)
			{
				if (screenCorners[i].Y < minY)
				{
					minY2 = minY;
					minY2Index = minYIndex;

					minY = screenCorners[i].Y;
					minYIndex = i;
				}
				else if (screenCorners[i].Y < minY2)
				{
					minY2 = screenCorners[i].Y;
					minY2Index = i;
				}
			}

			//find identified screen corners
			Vector2d TL = Vector2d.Zero;
			Vector2d TR = Vector2d.Zero;
			Vector2d BL = Vector2d.Zero;
			Vector2d BR = Vector2d.Zero;
			for (int i = 0; i < 4; i++)
			{
				if ((i == minXIndex || i == minX2Index) && (i == minYIndex || i == minY2Index))
					TL = screenCorners[i];
				else if ((!(i == minXIndex || i == minX2Index)) && (i == minYIndex || i == minY2Index))
					TR = screenCorners[i];
				else if ((i == minXIndex || i == minX2Index) && !(i == minYIndex || i == minY2Index))
					BL = screenCorners[i];
				else
					BR = screenCorners[i];
			}

			screenCorners[0] = TL;
			screenCorners[1] = TR;
			screenCorners[2] = BL;
			screenCorners[3] = BR;
			return screenCorners;
		}
		return null;
	}

	/// <summary>
	/// updates homographic matrix based on corner circle positions
	/// </summary>
	private void updateHomography()
	{
		HomFound = false;
		if (hCircles != null && hCircles.Count >= 4)
		{
			List<Vector2d> screenCorners = getCornerPoints();

			//normalized device coordinate corners
			NDCPoints = new List<Vector2d>();
			NDCPoints.Add(new Vector2d(-1, -1)); NDCPoints.Add(new Vector2d(1, -1));
			NDCPoints.Add(new Vector2d(-1, 1)); NDCPoints.Add(new Vector2d(1, 1));

			//find mapping from raster corners in video space to NDC corners
			HomMatrix = Matrix3.Homography(screenCorners, NDCPoints);

			HomFound = true;
		}
	}
	
	/*
	 * LASER POSITION RECTIFICATION
	 */
	/// <summary>
	/// rectify point positions from video space to NDC application space
	/// </summary>
	public override void rectifyLaserPositions()
	{
		base.rectifyLaserPositions();
		if (HomFound)
		{
			foreach (LaserDot laser in lasers)
				if (laser.onScreen)
					laser.AdjustedPosition = HomMatrix.transform(laser.AdjustedPosition);
		}
	}
}