using System;
using System.Collections.Generic;
using System.Linq;
using System.Drawing;
using System.Text;
using System.Threading.Tasks;
using Emgu.CV;
using Emgu.CV.Structure;

namespace bbiwarg.Utility
{
    class Projection2DTo2D
    {
        private ImageSize sizeA;
        private ImageSize sizeB;
        private List<PointF> calibrationPointsA;
        private List<PointF> calibrationPointsB;
        private int numPointsForCalibration;
        private HomographyMatrix homography;

        public bool IsCalibrated { get; private set; }

        public Projection2DTo2D(ImageSize sizeA, ImageSize sizeB, int numPointsForCalibration = 4) {
            this.sizeA = sizeA;
            this.sizeB = sizeB;
            this.numPointsForCalibration = numPointsForCalibration;

            reset();
        }

        public void reset() {
            homography = null;
            IsCalibrated = false;
            calibrationPointsA = new List<PointF>();
            calibrationPointsB = new List<PointF>();
        }

        public void addCalibrationPoints(Vector2D pointA, Vector2D pointB) {
            calibrationPointsA.Add(sizeA.getRelativePoint(pointA));
            calibrationPointsB.Add(sizeB.getRelativePoint(pointB));

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

        public Vector2D projectPoint(Vector2D pointA) {
            PointF[] pointfsB = new PointF[1] {sizeA.getRelativePoint(pointA)};
            homography.ProjectPoints(pointfsB);
            return sizeB.getAbsolutePoint(new Vector2D(pointfsB[0]));
        }

        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();
        }

        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);
        }
    }
}