//Using statements omitted

/// <summary>
/// This class stores the information regarding the calibration of a video capture source for tracking laser pointer dots or
/// "laser dots". This class performs the following functions:
/// * Crops the video stream to help optimize tracking
/// * Corrects tracked positions to account for lens distortion
/// * Stores reference brightness thresholding images to compare video frames against.
/// </summary>
public class CalibrationData
{
	/*
	* CONSTANTS
	*/
	/// <summary>
	/// the default pixel brightness threshold
	/// </summary>
	public const int DEFAULT_THRESHOLD = 500;
	/// <summary>
	/// the padding to add to the pixel brightness threshold calculation
	/// </summary>
	public const int THRESHOLD_PADDING = 20;
	/// <summary>
	/// the number of K vals needed to describe the camera lens
	/// </summary>
	public const int NUM_K_VALS = 2;

	/*
	* CLASS PROPERTIES
	*/
	/// <summary>
	/// the camera device name
	/// </summary>
	protected string cameraDeviceName;
	/// <summary>
	/// the capture device object
	/// </summary>
	protected CaptureDevice cameraDevice;

	/// <summary>
	/// the calibration form object
	/// </summary>
	protected CalibrationForm calibrationForm;
	/// <summary>
	/// the video monitor on the calibration form
	/// </summary>
	protected PictureBox videoMonitor;
	/// <summary>
	/// the last captured frame from the video
	/// for display to the video monitor
	/// </summary>
	protected Bitmap displayFrame;

	/// <summary>
	/// the full uncropped resolution of the video feed
	/// </summary>
	protected WinDraw.Size fullResolution;
	
	/// <summary>
	/// cropping rectangle
	/// </summary>
	protected WinDraw.Rectangle cropRectangle;

	//lens rectification
	//K lens parameters
	protected bool KValuesFound;
	protected DoubleVector Kvector;

	//normalization matrices
	protected Matrix3 tNormLens;
	protected Matrix3 tNormLensInv;

	/// <summary>
	/// point correction matrix to adjust for cropping
	/// </summary>
	protected Matrix3 tPointCorrection;

	/// <summary>
	/// the laser dot tracker object
	/// </summary>
	protected LaserDotTracking laserTracker;
	
	/// <summary>
	/// the brightness threshold level, above
	/// which pixels are considered to be a laser dot
	/// </summary>
	protected int brightnessThreshold;

	/// <summary>
	/// the number of lasers being tracked
	/// </summary>
	protected int numLasers;
	/// <summary>
	/// the list of laser dot objects
	/// </summary>
	protected List<LaserDot> lasers;

	//user point manipulation data omitted...

	/// <summary>
	/// constructor for a new video calibration 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 CalibrationData(string newCaptureDeviceName,
		WinDraw.Size videoResolution, PictureBox newMonitor,
		CaptureDevice newCaptureDevice, CalibrationForm newCalibrationForm)
	{
		//video feed
		calibrationForm = newCalibrationForm;
		videoMonitor = newMonitor;

		fullResolution = videoResolution;

		cameraDeviceName = newCaptureDeviceName;
		cameraDevice = newCaptureDevice;
		if (cameraDevice != null)
			cameraDevice.NewFrame += new CameraEventHandler(ProcessFrame);

		//lens correction
		KValuesFound = false;
		Kvector = new DoubleVector(NUM_K_VALS);
		updateTnorm();

		//default threshold
		brightnessThreshold = DEFAULT_THRESHOLD;

		//no cropping
		cropRectangle = new WinDraw.Rectangle(0, 0,
						fullResolution.Width, fullResolution.Height);
		tPointCorrection = Matrix3.Identity;

		//laser dots
		this.NumLasers = DEFAULT_NUM_LASERS;

		//create laserTracker
		laserTracker = new LaserDotTracking(this, lasers);
	}

	//encapsulators omitted

	/*
	* VIDEO PROCESSING
	*/
	/// <summary>
	/// should be called every CalibrationForm.UPDATES_PER_SECOND
	/// by the calibration form object
	/// </summary>
	public virtual void update()
	{
		//update display
		if (displayFrame != null && videoMonitor != null)
		{
			try
			{
				if (videoMonitor.Image != this.displayFrame)
				{
					if (videoMonitor.Image != null)
						videoMonitor.Image.Dispose();

					drawOnDisplayFrame();
					videoMonitor.Image = this.displayFrame;
				}
			}
			catch (InvalidOperationException exc)
			{
				Log(exc.Tostring());
			}
			catch (ArgumentException exc)
			{
				Log(exc.Tostring());
			}
		}
	}

	/// <summary>
	/// draw laser positions and crop box onto frame
	/// </summary>
	private void drawOnDisplayFrame()
	{
		try
		{
			Graphics dc = Graphics.FromImage(displayFrame);

			//draw cropping rectangle
			dc.DrawRectangle(new Pen(Color.White), cropRectangle);

			//add circle to image where laser positions were located
			if (laserTracker.NumVisibleLasers > 0)
			{
				Pen p = new Pen(Color.Red, 1);
				foreach (LaserDot laser in lasers)
					if (laser.onScreen)
						dc.DrawEllipse(p, (float)laser.NextPosition.X - 5,
							(float)laser.NextPosition.Y - 5, 10, 10);
			}
			dc.Dispose();
		}
		catch (OutOfMemoryException OoM)
		{
			Log(OoM.Tostring());
			GC.Collect(); //force garbage collection
		}
		catch (AccessViolationException exc)
		{
			Log(exc.Tostring());
			return;
		}
	}

	/// <summary>
	/// event listener to handle a new video frame from the camera
	/// </summary>
	protected virtual void ProcessFrame(object sender, CameraEventArgs e)
	{
		if (videoMonitor.Image != displayFrame)
			displayFrame.Dispose();

		//frame for display to the video monitor
		displayFrame = (Bitmap)e.Bitmap.Clone();

		//process frame to identify new laser dot positions
		laserTracker.ProcessFrame((Bitmap)e.Bitmap.Clone(cropRectangle,
								e.Bitmap.PixelFormat));
		
		//rectify positions from video space to NDC application space
		rectifyLaserPositions();
	}

	/// <summary>
	/// stops video feed processsing
	/// </summary>
	public void stopAll()
	{
		if (cameraDevice != null)
		{
			cameraDevice.NewFrame -= new CameraEventHandler(ProcessFrame);
			cameraDevice.Stop();
		}
		laserTracker.unload();
	}

	/// <summary>
	/// rectififies the identified laser dot positions
	/// from video raster space to NDC application space
	/// </summary>
	public virtual void rectifyLaserPositions()
	{
		foreach(LaserDot laser in lasers)
		{
			if (laser.onScreen)
			{
				//lens correction...
				if (KValuesFound)
					laser.AdjustedPosition = tNormLensInv.transform(
									radialDistort(
									tNormLens.transform(
									laser.NextPosition), 
									Kvector));
				else
					laser.AdjustedPosition = laser.NextPosition;
			}
		}
	}

	/*
	* AUTO CALLIBRATION
	*/
	public virtual void AutoUpdateCorners()
	{
		//overriden by planar calibration
	}
	
	/*
	* CROPPING
	*/
	/// <summary>
	/// crop window to corners of screen area
	/// (overriden by planar calibration)
	/// </summary>
	public virtual void AutoCrop()
	{
		//crop the brightnessThreshold image
	   if (laserTracker.ThresholdImage != null)
			laserTracker.ThresholdImage = laserTracker.ThresholdImage.Clone(
			cropRectangle, laserTracker.ThresholdImage.PixelFormat); 

		//update callibration form
		if (calibrationForm != null)
			calibrationForm.updateCrop(cropRectangle.Top,
						fullResolution.Width - cropRectangle.Right + 1,
						fullResolution.Height - cropRectangle.Bottom + 1,
						cropRectangle.Left);
	}

	/// <summary>
	/// gets / sets the current video stream cropping rectangle
	/// </summary>
	public WinDraw.Rectangle CroppingRectangle
	{
		get { return cropRectangle; }
		set { cropRectangle = value; updateCrop(); }
	}

	/// <summary>
	/// updates the cropped display
	/// </summary>
	public void updateCrop()
	{
		//videoMonitor.Location = new Point(cropRectangle.X, cropRectangle.Y);
		if (laserTracker != null)
			laserTracker.Reset();
		//update the matrix which corrects video positions to adjust for cropping
		tPointCorrection = Matrix3.Identity;
		tPointCorrection.translate(cropRectangle.X, cropRectangle.Y);
	}

	/// <summary>
	/// sets the last captured frame to be the brightnessThreshold image
	/// </summary>
	public void updateThresholdImage()
	{
		if (this.displayFrame != null)
		{
			//save threshold image for testing purposes
			displayFrame.Save("./" + CalibrationData.THRESHOLD_IMAGE_NAME,
							WinImage.ImageFormat.Jpeg);
			//cropped brightnessThreshold image
			laserTracker.ThresholdImage = displayFrame.Clone(cropRectangle,
							displayFrame.PixelFormat);
		}
	}

	/// <summary>
	/// uses the brightnessThreshold frame
	///(an image of the screen at 50% brightness)
	/// and the max brightness frame 
	/// (an image with the screen at 100% brightness)
	/// to derive a reasonable brightness brightnessThreshold,
	/// above which pixels are considered to
	/// be a laser pointer dot
	/// </summary>
	public void updateThresholdLimit()
	{
		if (this.laserTracker == null || t
				his.laserTracker.ThresholdImage == null)
			return;

		UnsafeBitmap thresholdBitmap = new UnsafeBitmap((Bitmap)laserTracker.ThresholdImage);
		UnsafeBitmap maxBrightBitmap = new UnsafeBitmap((Bitmap)displayFrame.Clone(
					this.cropRectangle, this.laserTracker.ThresholdImage.PixelFormat));

		maxBrightBitmap.Bitmap.Save("./" + CalibrationData.MAX_THRESHOLD_IMAGE_NAME,
									WinImage.ImageFormat.Jpeg);
		
		thresholdBitmap.LockBitmap();
		maxBrightBitmap.LockBitmap();

		if (thresholdBitmap.Bitmap.Width != maxBrightBitmap.Bitmap.Width ||
			thresholdBitmap.Bitmap.Height != maxBrightBitmap.Bitmap.Height)
		{
			throw new Exception("raster image size mismatch");
		}

		// find the maximum difference between the max
		// brightness image and the thresholding image
		int maxBrightness = 0;
		
		for (int i = 0; i < maxBrightBitmap.Bitmap.Height; i++)
		{
			for (int j = 0; j < maxBrightBitmap.Bitmap.Width; j++)
			{
				PixelData brightPixel = maxBrightBitmap.GetPixel(j, i);
				PixelData thresPixel = thresholdBitmap.GetPixel(j, i);

				int ired = brightPixel.red - thresPixel.red;
				int igreen = brightPixel.green - thresPixel.green;
				int iblue = brightPixel.blue - thresPixel.blue;

				if(ired < 0)
					thresPixel.red = 0;
				else
					thresPixel.red = (byte)ired;

				if(igreen < 0)
					thresPixel.green = 0;
				else
					thresPixel.green = (byte)igreen;

				if(iblue < 0)
					thresPixel.blue = 0;
				else
					thresPixel.blue = (byte)iblue;

				int curThreshold = (thresPixel.red * LaserDotTracking.RED_MODIFIER) +
						   (thresPixel.green * LaserDotTracking.GREEN_MODIFIER) +
						   (thresPixel.blue * LaserDotTracking.BLUE_MODIFIER);

				if (curThreshold > maxBrightness)
					maxBrightness = curThreshold;
			}
		}

		//set the brightnessThreshold padding based
		//on the maximum brightness difference
		this.Threshold = maxBrightness + THRESHOLD_PADDING;
		calibrationForm.setThreshold(this.Threshold);
	}

	//user grid point manipulation omitted...

	/*
	* LENS RECTIFICATION
	*/
	/// <summary>
	/// updates the normalization matrix based on the video feed's width and height
	/// </summary>
	private void updateTnorm()
	{
		//normalization matrix places origin at the center of the video
		tNormLensInv = Matrix3.Identity;

		//translation
		tNormLensInv[0, 2] = (float)fullResolution.Width / 2;
		tNormLensInv[1, 2] = (float)fullResolution.Height / 2;

		//scaling
		tNormLensInv[0, 0] = (float)(fullResolution.Width + fullResolution.Height);
		tNormLensInv[1, 1] = (float)(fullResolution.Width + fullResolution.Height);

		tNormLensInv[2, 2] = 1.0f;
		tNormLens = tNormLensInv.returnInverse();
	}
	
	/// <summary>
	/// get the square of the distance from a test point to a line defined
	/// by a point on the line and a direction and the line length squared (for performance)
	/// </summary>
	public double distanceSquaredToLine(Vector2d lineStart, Vector2d lineDir,
							double lengthSqr, Vector2d testPoint)
	{
		// vector from line's start to test point
		Vector2d w = testPoint - lineStart;

		// find perpendicular projection of hit point onto line segment
		double r = (w * lineDir) / lengthSqr;

		// closest point is at parameter r distance from start
		return Vector2d.DistanceSquared(testPoint, lineStart + (r * lineDir));
	}

	/// <summary>
	/// get the distance from a test point to a line defined
	/// by a point and a line direction and the line length squared (for performance)
	/// </summary>
	public double distanceToLine(Vector2d lineStart, Vector2d lineDir,
						double lengthSqr, Vector2d testPoint)
	{
		return Math.Sqrt(distanceSquaredToLine(lineStart, lineDir, lengthSqr, testPoint));
	}

	/// <summary>
	/// get the radially distorted point position from a current point and a K matrix
	/// this method assumes the point has been normalized to a central origin
	/// </summary>
	public Vector2d radialDistort(Vector2d curP, DoubleVector Kvals)
	{
		double r = curP.Length();
		double Lr = 1;

		for (int i = 0; i < Kvals.Length; i++)
			Lr += Math.Pow(r, i + 1) * Kvals[i];

		return Lr * curP;
	}

	/// <summary>
	/// determines the error 
	/// </summary>
	/// <param name="x">the K vector that defines the lens distortion</param>
	/// <param name="count">the iteration count</param>
	/// <param name="maxIter">the total number of iterations</param>
	/// <returns>the residual error</returns>
	private double determineError(DoubleVector Kvals, int count, int maxIter)
	{
		double sum = 0;

		Vector2d correctLine = Vector2d.Zero;
		Vector2d lineStart = Vector2d.Zero;
		Vector2d lineEnd = Vector2d.Zero;

		double lengthSqr = 0;

		//lines defined by grid rows
		for (int i = 0; i < numGridRows; i++)
		{
			//rectified start and end points of grid row
			lineStart = radialDistort(gridPoints[i, 0], Kvals);
			lineEnd = radialDistort(gridPoints[i, numGridCols - 1], Kvals);

			//corrected line length and vector
			correctLine = lineEnd - lineStart;
			lengthSqr = correctLine.LengthSquared();

			//use all points in row except far left and right
			for (int j = 1; j < numGridCols-1; j++)
				//distance from staright line to test point
				sum += distanceSquaredToLine(lineStart, correctLine, lengthSqr,
									radialDistort(gridPoints[i, j], Kvals));
		}
		//lines defined by grid columns
		for (int j = 0; j < numGridCols; j++)
		{
			//rectified start and end points of grid column
			lineStart = radialDistort(gridPoints[0, j], Kvals);
			lineEnd = radialDistort(gridPoints[numGridRows - 1, j], Kvals);

			//corrected line length and vector
			correctLine = lineEnd - lineStart;
			lengthSqr = correctLine.LengthSquared();

			//all points in column except top and bottom
			for (int i = 1; i < numGridRows - 1; i++)
				//distance from staright line to test point
				sum += distanceSquaredToLine(lineStart, correctLine, lengthSqr,
									radialDistort(gridPoints[i, j], Kvals));
		}
		return sum;
	}

	/// <summary>
	/// determine appropriate K values to adjust for lens distortion
	/// </summary>
	public void LensDistortionCorrection()
	{
		gridPoints = new Vector2d[numGridRows, numGridCols];

		for (int i = 0; i < numGridRows; i++)
			for (int j = 0; j < numGridCols; j++)
				gridPoints[i, j] = tNormLens * new Vector2d(gridCircles[i, j].Location);

		Kvector = new DoubleVector(NUM_K_VALS);

		//test error when fixing barrel distortion
		FloatMatrix.setAllValues(ref Kvector, 0.1);
		double barrelDistortionError = determineError(Kvector, 0, 200);

		//test error when fixing pin cushion distortion
		FloatMatrix.setAllValues(ref Kvector, -0.1);
		double pinCushionDistortionError = determineError(Kvector, 0, 200);

		//use more accurate model
		if(barrelDistortionError < pinCushionDistortionError)
			FloatMatrix.setAllValues(ref Kvector, 0.1);

		double minimizedError = 0;
		string outmsg = "";

		//two methods for iteratively solving for lens distortion parameters
		//SimpleIterative uses a method of my own design, LMFsolve uses the Levenberg-Marquardt method
		Kvector = FloatMatrix.SimpleIterative(Kvector, new LFMResiduals(determineError),
									1E-12, ref outmsg, ref minimizedError, 50, 10);
		//Kvector = FloatMatrix.LMFsolve(Kvector, new LFMResiduals(determineError), ref outmsg,
							//ref numIterations, ref minimizedError, 50, 1e-12, 1e-12, 10);
		Log(outmsg);

		int numPoints = numGridRows * numGridCols;
		double initialError = determineError(new DoubleVector(Kvector.Length), 0, 1);
		double residualError = determineError(this.Kvector, 0, 1);
		
		Log("initial error: " + initialError + "  residual error: " + residualError);

		KValuesFound = true;
	}
}