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.GetPerspectiveTransform(calibrationPointsA.ToArray(), calibrationPointsB.ToArray());
            homography = CameraCalibration.FindHomography(calibrationPointsA.ToArray(), calibrationPointsB.ToArray(), Emgu.CV.CvEnum.HOMOGRAPHY_METHOD.DEFAULT, 0.995);
            IsCalibrated = true;
        }
    }
}