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(pointA);
            calibrationPointsB.Add(pointB);

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

        public Vector2D projectPoint(Vector2D pointA) {
            PointF[] pointfsB = new PointF[1] {pointA};
            homography.ProjectPoints(pointfsB);
            return 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;

            Console.WriteLine("HOMOGRAPHY:");
            for (int r = 0; r < homography.Size.Height; r++) {
                for (int c = 0; c < homography.Size.Width; c++) {
                    Console.Write(homography.Data[r, c] + "\t");
                }
                Console.WriteLine();
            }
        }
    }
}