using Aurora;
using Aurora.Api;
using Aurora.Api.DataType;
using VisionCommon;
using System;
using System.Collections.Generic;
using System.Reactive.Linq;

namespace VisionCalibrator {
    /// <summary>
    /// 이미지 좌표와 자재 좌표간의 변환을 위한 클래스이다.
    /// </summary>
    public class Calibrator : IDisposable {
        private IEventObservable _EventObservable;
        private Logger _Logger;
        private FovInfo _FovInfo;
        private SubSysInfo _SubSysInfo;
        private CalRecipe _CalRecipe;
        private FovRecipe _FovRecipe;
        private SetupDirection CamSetupDirection = new SetupDirection();
        private FlipAndRotation FlipAndRotation = new FlipAndRotation();

        /// <summary>
        /// Calibrator 생성자
        /// </summary>
        /// <param name="fovInfo">Fov 정보</param>
        public Calibrator(FovInfo fovInfo) {
            _EventObservable = VisionEnv.AuroraEnv.EventObservable;
            _FovInfo = fovInfo;
            _SubSysInfo = fovInfo.ToSubSysInfo();
            _Logger = new Logger(_FovInfo.SubSysName + _FovInfo.FovName);
            UpdateRecipe();
            UpdateCameraConfig();
            SetObservable();
        }

        private void SetObservable() {
            var observable = VisionEventId.GetRecipeUpdateObservable();
            observable.Subscribe(_ => UpdateRecipe());

            Observable.Merge(
                VisionEnv.AuroraEnv.EventObservable.AsObservable(AuroraSysEvent.SYS_DEVICE_ACTIVATED))
                .Where(x => _FovRecipe != null)
                .Where(x => x.Param as string == _FovRecipe.CamRecipe.CamId.ToString())
                .Subscribe(_ => UpdateCameraConfig());
        }

        /// <summary>
        /// 이미지 좌표를 자재 좌표계로 변환한다
        /// </summary>
        /// <param name="imagePoint">변환할 이미지 좌표</param>
        /// <param name="imageSize">변환할 이미지의 크기</param>
        /// <returns>자재 좌표</returns>
        public StaticPoint TransformToCalibratedSpace(ImagePoint imagePoint, ImageSize imageSize) {
            if (_FovRecipe == null) {
                return new StaticPoint();
            }
            var fovCenter = _FovRecipe.CalFovRecipe.FovCenter;
            return TransformToCalibratedSpace(fovCenter, imagePoint, imageSize.Width, imageSize.Height);
        }

        /// <summary>
        /// 이미지 좌표를 자재 좌표계로 변환한다
        /// </summary>
        /// <param name="imagePoint">변환할 이미지 좌표</param>
        /// <param name="imageSize">변환할 이미지의 크기</param>
        /// <param name="errorRotation">변환할 이미지의 회전 오차</param>
        /// <param name="pixelSize">카메라의 픽셀 크기</param>
        /// <returns>자재 좌표</returns>
        public StaticPoint TransformToCalibratedSpace(ImagePoint imagePoint, ImageSize imageSize, Angle errorRotation, StaticPoint pixelSize) {
            var fovCenter = _FovRecipe.CalFovRecipe.FovCenter;
            return TransformToCalibratedSpace(fovCenter, imagePoint, imageSize.Width, imageSize.Height, errorRotation, pixelSize);
        }

        /// <summary>
        /// X축 픽셀 사이즈만으로 이미지 좌표를 자재 좌표계로 변환한다
        /// </summary>
        /// <param name="imagePoint">변환할 이미지 좌표</param>
        /// <param name="imageSize">변환할 이미지의 크기</param>
        /// <param name="errorRotation">변환할 이미지의 회전 오차</param>
        /// <param name="pixelSizeX">카메라의 X축 픽셀 크기</param>
        /// <returns>자재 좌표</returns>
        public StaticPoint TransformToCalibratedSpaceWithPixelX(ImagePoint imagePoint, ImageSize imageSize, Angle errorRotation, double pixelSizeX) {
            var fovCenter = _FovRecipe.CalFovRecipe.FovCenter;
            return TransformToCalibratedSpaceWithPixelX(fovCenter, imagePoint, imageSize.Width, imageSize.Height, errorRotation, pixelSizeX);
        }

        /// <summary>
        /// Y축 픽셀 사이즈만으로 이미지 좌표를 자재 좌표계로 변환한다
        /// </summary>
        /// <param name="imagePoint">변환할 이미지 좌표</param>
        /// <param name="imageSize">변환할 이미지의 크기</param>
        /// <param name="errorRotation">변환할 이미지의 회전 오차</param>
        /// <param name="pixelSizeY">카메라의 Y축 픽셀 크기</param>
        /// <returns>자재 좌표</returns>
        public StaticPoint TransformToCalibratedSpaceWithPixelY(ImagePoint imagePoint, ImageSize imageSize, Angle errorRotation, double pixelSizeY) {
            var fovCenter = _FovRecipe.CalFovRecipe.FovCenter;
            return TransformToCalibratedSpaceWithPixelY(fovCenter, imagePoint, imageSize.Width, imageSize.Height, errorRotation, pixelSizeY);
        }

        internal StaticPoint TransformToCalibratedSpaceWithPixelX(
            StaticPoint fovCenter, ImagePoint imagePoint, int imageWidth, int imageHeight, Angle errorRotation, double pixelSizeX) {
            // Pixel size [px -> mm]
            var recipePixelSize = _FovRecipe.CalFovRecipe.PixelSize;

            StaticPoint pixelSize = new StaticPoint(pixelSizeX, recipePixelSize.Y);

            return TransformToCalibratedSpace(
                fovCenter,
                imagePoint,
                imageWidth,
                imageHeight,
                errorRotation,
                pixelSize);
        }

        internal StaticPoint TransformToCalibratedSpaceWithPixelY(
            StaticPoint fovCenter, ImagePoint imagePoint, int imageWidth, int imageHeight, Angle errorRotation, double pixelSizeY) {
            // Pixel size [px -> mm]
            var recipePixelSize = _FovRecipe.CalFovRecipe.PixelSize;

            StaticPoint pixelSize = new StaticPoint(recipePixelSize.X, pixelSizeY);

            return TransformToCalibratedSpace(
                fovCenter,
                imagePoint,
                imageWidth,
                imageHeight,
                errorRotation,
                pixelSize);
        }

        internal StaticPoint TransformToCalibratedSpace(
            StaticPoint fovCenter, ImagePoint imagePoint, int imageWidth, int imageHeight) {
            // 카메라 설치 오차 보정 값
            var errorRotation = _FovRecipe.CalFovRecipe.ErrRotationX.Clone() as Angle;

            // Pixel size [px -> mm]
            var pixelSize = _FovRecipe.CalFovRecipe.PixelSize;

            return TransformToCalibratedSpace(
                fovCenter,
                imagePoint,
                imageWidth,
                imageHeight,
                errorRotation,
                pixelSize);
        }

        internal StaticPoint TransformToCalibratedSpace(
            StaticPoint fovCenter, ImagePoint imagePoint, int imageWidth, int imageHeight, Angle errorRotation, StaticPoint pixelSize) {
            try {
                // FlipY 변환 값
                eFlipY flipY = GetCorrectionFlipY();

                // RotationX 변환 값
                var setupDirection = GetCorrectionRotationX();

                var fovCenterOffset = _FovRecipe.CalFovRecipe.FovCenterOffset;

                var res = TransformImageToStatic(
                    fovCenter,
                    fovCenterOffset,
                    imagePoint,
                    setupDirection, // rotCorrectionX, rotErrorX
                    errorRotation,
                    imageWidth,
                    imageHeight,
                    (int)flipY,
                    pixelSize.X,
                    pixelSize.Y);
                return res;
            }
            catch (Exception e) {
                _Logger.FuncE(e.Message);
            }
            return null;
        }

        private static StaticPoint TransformImageToStatic(
            StaticPoint fovCenter,
            StaticPoint fovCenterOffset,
            ImagePoint imagePoint,
            Angle setupDirection, Angle errorRotation,
            int imageWidth, int imageHeight,
            int flipY, double pixelSizeX, double pixelSizeY) {
            try {
                var FovCenter = new PointData() {
                    X = fovCenter.X,
                    Y = fovCenter.Y
                };

                var FovCenterOffset = new PointData() {
                    X = fovCenterOffset.X,
                    Y = fovCenterOffset.Y
                };

                var ImagePos = new PointData() {
                    X = imagePoint.X,
                    Y = imagePoint.Y
                };

                var TargetPos = NativeCalibrator.ImageToStatic(FovCenter, FovCenterOffset,
                    setupDirection.Radian, errorRotation.Radian,
                    imageWidth, imageHeight,
                    flipY, pixelSizeX, pixelSizeY, ImagePos);

                return TargetPos;
            }
            catch (Exception e) {
            }
            return new StaticPoint();
        }

        /// <summary>
        /// 자재 좌표를 이미지 좌표계로 변환한다
        /// </summary>
        /// <param name="staticPoint">변환할 자재 좌표</param>
        /// <param name="imageSize">변환할 이미지의 크기</param>
        /// <returns>이미지 좌표</returns>
        public ImagePoint TransformToImagePoint(StaticPoint staticPoint, ImageSize imageSize) {
            return InverseTransformToCalibratedSpace(
                _FovRecipe.CalFovRecipe.FovCenter,
                staticPoint,
                imageSize.Width,
                imageSize.Height,
                _FovRecipe.CalFovRecipe.ErrRotationX,
                _FovRecipe.CalFovRecipe.PixelSize
                );
        }

        internal ImagePoint InverseTransformToCalibratedSpace(
            StaticPoint fovCenter, StaticPoint staticPoint, int imageWidth, int imageHeight, Angle errorRotation, StaticPoint pixelSize) {
            try {
                // FlipY 변환 값
                eFlipY flipY = GetCorrectionFlipY();

                // RotationX 변환 값
                var setupDirection = GetCorrectionRotationX();

                var fovCenterOffset = _FovRecipe.CalFovRecipe.FovCenterOffset;

                var res = TransformStaticToImage(
                    fovCenter,
                    fovCenterOffset,
                    staticPoint,
                    setupDirection, // rotCorrectionX, rotErrorX
                    errorRotation,
                    imageWidth,
                    imageHeight,
                    (int)flipY,
                    pixelSize.X,
                    pixelSize.Y);
                return res;
            }
            catch (Exception e) {
                _Logger.FuncE(e.Message);
            }
            return null;
        }

        private static ImagePoint TransformStaticToImage(
            StaticPoint fovCenter,
            StaticPoint fovCenterOffset,
            StaticPoint staticPoint,
            Angle setupDirection, Angle errorRotation,
            int imageWidth, int imageHeight,
            int flipY, double pixelSizeX, double pixelSizeY) {
            try {
                var FovCenter = new PointData() {
                    X = fovCenter.X,
                    Y = fovCenter.Y
                };

                var FovCenterOffset = new PointData() {
                    X = fovCenterOffset.X,
                    Y = fovCenterOffset.Y
                };

                var StaticPos = new PointData() {
                    X = staticPoint.X,
                    Y = staticPoint.Y
                };

                var TargetPos = NativeCalibrator.StaticToImage(FovCenter, FovCenterOffset,
                    setupDirection.Radian, errorRotation.Radian,
                    imageWidth, imageHeight,
                    flipY, pixelSizeX, pixelSizeY, StaticPos);

                return TargetPos;
            }
            catch (Exception e) {
            }
            return new ImagePoint();
        }

        //View Model에서 호출
        /// <summary>
        /// 카메라의 픽셀 크기를 계산한다.
        /// </summary>
        /// <param name="meaImagePoints">이미지 좌표 리스트</param>
        /// <param name="moveLength">측정시 이동 거리</param>
        /// <param name="errorRot">회전 오차</param>
        /// <param name="target">계산할 픽셀 크기의 축</param>
        /// <returns>픽셀 크기</returns>
        public double GetPixelSize(List<ImagePoint> meaImagePoints, double moveLength, double errorRot, char target) {
            // target parameter ErrorRot enum 활용
            double pixelSize = 0;
            pixelSize = GetPixelSizeInternal(meaImagePoints, moveLength, errorRot, target);

            return pixelSize;
        }
        private static double GetPixelSizeInternal(List<ImagePoint> meaImagePoints, double moveLength, double errorRot, char target) {
            return NativeCalibrator.CalibrateImagePixel(meaImagePoints, moveLength, errorRot, target);
        }

        //View Model에서 호출
        /// <summary>
        /// 회전 오차를 계산한다.
        /// </summary>
        /// <param name="meaPoints">자재 좌표 리스트</param>
        /// <param name="target">계산시 사용할 데이터의 축</param>
        /// <returns>회전 오차</returns>
        public Angle FindStaticErrorRotation(List<ImagePoint> meaPoints, char target) {
            List<ImagePoint> meaPointsY = new List<ImagePoint>();
            foreach (var staticPoint in meaPoints) {
                _Logger.FuncI($"meaPoints Data : {staticPoint}");
                if (target == 'y') {
                    meaPointsY.Add(new ImagePoint(staticPoint.Y, staticPoint.X * -1));
                }
            }
            if (target == 'y') {
                return FindStaticErrorRotationInternal(meaPointsY);
            }

            return FindStaticErrorRotationInternal(meaPoints);
        }
        private static Angle FindStaticErrorRotationInternal(List<ImagePoint> meaPoints) {
            return NativeCalibrator.StaticCalibrateErrorRot(meaPoints);
        }

        //View Model에서 호출
        /// <summary>
        /// 회전 원점을 계산한다.
        /// </summary>
        /// <param name="measuredData">자재 좌표 리스트</param>
        /// <param name="planRotation">회전한 각도</param>
        /// <returns>회전 원점의 자재 좌표</returns>
        public StaticPoint GetThetaOriginPoint(List<StaticPoint> measuredData, Angle planRotation) {
            _Logger.FuncI($"Plan Rotation : {planRotation}");
            Angle rotation = CoordSystem.DenormalizeAngleDirection(planRotation, (int)_CalRecipe.CoordSystem.FlipY);
            StaticPoint thetaOrigin = GetThetaOriginPointInternal(measuredData, rotation);

            return thetaOrigin;
        }

        private static StaticPoint GetThetaOriginPointInternal(List<StaticPoint> measuredData, Angle rotation) {
            StaticPoint thetaOrigin = NativeCalibrator.StaticCalibrateThetaOrigin(measuredData, rotation);

            return thetaOrigin;
        }

        /// <summary>
        /// Fov 레시피를 새로 가져온다.
        /// </summary>
        /// <param name="fovRecipe">Fov 레시피</param>
        public void RefreshFovRecipe(FovRecipe fovRecipe) {
            _FovRecipe = fovRecipe;
        }

        private void UpdateRecipe() {
            _CalRecipe = VisionRecipeManager.GetCalRecipe(_SubSysInfo);
            _FovRecipe = VisionRecipeManager.GetFovRecipe(_FovInfo);
        }

        private void UpdateCameraConfig() {
            var camRecipe = VisionRecipeManager.GetCamRecipe(_FovInfo);
            if (camRecipe != null) {
                var CameraConfig = VisionEnv.AuroraEnv.DeviceManager.GetDeviceConfig(camRecipe.CamId.ToString()) as CamConfig;
                CamSetupDirection = CameraConfig.SetupDirection.Clone() as SetupDirection;
                FlipAndRotation = CameraConfig.FlipAndRotation.Clone() as FlipAndRotation;
            }
        }

        internal Angle GetCorrectionRotationX() {
            return Angle.CreateWithDegree(
                (_CalRecipe.CoordSystem.RotationX.Degree - CamSetupDirection.RotationX) * (int)CamSetupDirection.FlipY);
        }

        internal eFlipY GetCorrectionFlipY() {
            return (eFlipY)((int)_CalRecipe.CoordSystem.FlipY * (int)CamSetupDirection.FlipY);
        }

        /// <summary>
        /// 좌표계에 Y축 반전을 적용 여부를 가져온다.
        /// </summary>
        /// <returns>Y축 반전 여부</returns>
        public eFlipY GetFlipY() {
            return _CalRecipe.CoordSystem.FlipY;
        }

        public void Dispose() {
        }
    }
}