using Emgu.CV;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Text;

namespace BBIWARG.Utility
{
    /// <summary>
    /// Computes and stores a homography matrix and provides functions to export it and project points.
    /// </summary>
    internal class Projection2DTo2D
    {
        /// <summary>
        /// calibration points in the first image (match points in calibrationPointsB)
        /// </summary>
        private List<PointF> calibrationPointsA;

        /// <summary>
        /// calibration points in the second image (match points in calibrationPointsA)
        /// </summary>
        private List<PointF> calibrationPointsB;

        /// <summary>
        /// homography matrix used to compute the projected points
        /// </summary>
        private HomographyMatrix homography;

        /// <summary>
        /// number of points used for the calibration
        /// </summary>
        private int numPointsForCalibration;

        /// <summary>
        /// size of the image the original points are in
        /// </summary>
        private ImageSize sizeA;

        /// <summary>
        /// size of the image the projected points are in
        /// </summary>
        private ImageSize sizeB;

        /// <summary>
        /// true iff the calibration is finished
        /// </summary>
        public bool IsCalibrated { get; private set; }

        /// <summary>
        /// Constructs a Projection2DTo2D.
        /// </summary>
        /// <param name="sizeA">size of the image the original points are in</param>
        /// <param name="sizeB">size of the image the projected points are in</param>
        /// <param name="numPointsForCalibration">number of points used for the calibration</param>
        public Projection2DTo2D(ImageSize sizeA, ImageSize sizeB, int numPointsForCalibration = 4)
        {
            this.sizeA = sizeA;
            this.sizeB = sizeB;
            this.numPointsForCalibration = numPointsForCalibration;

            reset();
        }

        /// <summary>
        /// Adds a pair of calibration points.
        /// </summary>
        /// <param name="pointA">point in the first image</param>
        /// <param name="pointB">point in the second image</param>
        public void addCalibrationPoints(Vector2D pointA, Vector2D pointB)
        {
            calibrationPointsA.Add(sizeA.getRelativePoint(pointA));
            calibrationPointsB.Add(sizeB.getRelativePoint(pointB));

            if (calibrationPointsA.Count == numPointsForCalibration)
                calibrate();
        }

        /// <summary>
        /// Projects a point.
        /// </summary>
        /// <param name="pointA">the point to project</param>
        /// <returns>projected point</returns>
        public Vector2D projectPoint(Vector2D pointA)
        {
            PointF[] pointfsB = new PointF[1] { sizeA.getRelativePoint(pointA) };
            homography.ProjectPoints(pointfsB);
            return sizeB.getAbsolutePoint(new Vector2D(pointfsB[0]));
        }

        /// <summary>
        /// Resets the calibration.
        /// </summary>
        public void reset()
        {
            homography = null;
            IsCalibrated = false;
            calibrationPointsA = new List<PointF>();
            calibrationPointsB = new List<PointF>();
        }

        /// <summary>
        /// Computes the homography from the lists of calibration points.
        /// </summary>
        private void calibrate()
        {
            homography = CameraCalibration.FindHomography(calibrationPointsA.ToArray(), calibrationPointsB.ToArray(), Emgu.CV.CvEnum.HOMOGRAPHY_METHOD.DEFAULT, 0.995);

            calibrationPointsA.Clear();
            calibrationPointsB.Clear();
            IsCalibrated = true;

            exportHomography();
        }

        /// <summary>
        /// Writes the homography to a file.
        /// </summary>
        private void exportHomography()
        {
            String[] fileData = new String[homography.Size.Height];
            StringBuilder sb = new StringBuilder();
            for (int r = 0; r < homography.Size.Height; r++)
            {
                for (int c = 0; c < homography.Size.Width; c++)
                {
                    sb.Append(homography.Data[r, c]);
                    sb.Append(" ");
                }
                fileData[r] = sb.ToString();
                sb.Clear();
            }
            System.IO.File.WriteAllLines(Parameters.HomographyFileName, fileData);
        }
    }
}