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

namespace VisionCalibrator {
    /// <summary>
    /// 자재 좌표를 모션 좌표로 변환하기 위한 클래스이다.
    /// </summary>
    public class CoordConverter {
        private static Logger _StaticLogger = new Logger("StaticCoordConverter");
        private Logger _Logger;
        private IEventObservable _EventObservable;
        private SubSysInfo _SubSysInfo;
        private CalRecipe _CalRecipe;
        private CoordConvRecipe _CoordConvRecipe;

        /// <summary>
        /// CoordConverter 생성자
        /// </summary>
        /// <param name="subSysInfo">서브 시스템 정보</param>
        public CoordConverter(SubSysInfo subSysInfo) {
            _SubSysInfo = subSysInfo;
            _Logger = new Logger(_SubSysInfo.SubSysName);
            _EventObservable = VisionEnv.AuroraEnv.EventObservable;
            UpdateRecipe();

            var observable = VisionEventId.GetRecipeUpdateObservable();
            observable.Subscribe(_ => UpdateRecipe());
        }

        /// <summary>
        /// 리스트로 전달된 자재 좌표를 모션 좌표로 변환한다.
        /// </summary>
        /// <param name="staticPoints">자재 좌표 리스트</param>
        /// <param name="horiozntalDistance">패턴 간의 수평 거리</param>
        /// <param name="verticalDistance">패턴 간의 수직 거리</param>
        /// <param name="llMotion">수평 Low,  수직 Low  모션 좌표</param>
        /// <param name="hlMotion">수평 High, 수직 Low  모션 좌표</param>
        /// <param name="lhMotion">수평 Low,  수직 High 모션 좌표</param>
        /// <param name="hhMotion">수평 High, 수직 High 모션 좌표</param>
        /// <returns>모션 좌표 리스트</returns>
        public List<MotionPoint> CoordConvert(
            List<StaticPoint> staticPoints,
            double horiozntalDistance, double verticalDistance,
            MotionPoint llMotion, MotionPoint hlMotion, MotionPoint lhMotion, MotionPoint hhMotion) {
            List<MotionPoint> motionPoints = new List<MotionPoint>();

            foreach (var staticPoint in staticPoints) {
                motionPoints.Add(
                    CoordConvert(
                        staticPoint.X, staticPoint.Y,
                        horiozntalDistance, verticalDistance,
                        llMotion, hlMotion, lhMotion, hhMotion));
            }

            return motionPoints;
        }

        /// <summary>
        /// 자재 좌표를 모션 좌표로 변환한다.
        /// </summary>
        /// <param name="staticPoint">자재 좌표</param>
        /// <param name="horiozntalDistance">패턴 간의 수평 거리</param>
        /// <param name="verticalDistance">패턴 간의 수직 거리</param>
        /// <param name="llMotion">수평 Low,  수직 Low  모션 좌표</param>
        /// <param name="hlMotion">수평 High, 수직 Low  모션 좌표</param>
        /// <param name="lhMotion">수평 Low,  수직 High 모션 좌표</param>
        /// <param name="hhMotion">수평 High, 수직 High 모션 좌표</param>
        /// <returns>모션 좌표</returns>
        public MotionPoint CoordConvert(
            StaticPoint staticPoint,
            double horiozntalDistance, double verticalDistance,
            MotionPoint llMotion, MotionPoint hlMotion, MotionPoint lhMotion, MotionPoint hhMotion) {

            return CoordConvert(
                staticPoint.X, staticPoint.Y,
                horiozntalDistance, verticalDistance,
                llMotion, hlMotion, lhMotion, hhMotion);
        }

        /// <summary>
        /// 자재 좌표를 모션 좌표로 변환한다.
        /// </summary>
        /// <param name="staticPointX">자재 X좌표</param>
        /// <param name="staticPointY">자재 Y좌표</param>
        /// <param name="horiozntalDistance">패턴 간의 수평 거리</param>
        /// <param name="verticalDistance">패턴 간의 수직 거리</param>
        /// <param name="llMotion">수평 Low,  수직 Low  모션 좌표</param>
        /// <param name="hlMotion">수평 High, 수직 Low  모션 좌표</param>
        /// <param name="lhMotion">수평 Low,  수직 High 모션 좌표</param>
        /// <param name="hhMotion">수평 High, 수직 High 모션 좌표</param>
        /// <returns>모션 좌표</returns>
        public MotionPoint CoordConvert(
            double staticPointX, double staticPointY,
            double horiozntalDistance, double verticalDistance,
            MotionPoint llMotion, MotionPoint hlMotion, MotionPoint lhMotion, MotionPoint hhMotion) {

            var translatedPoint = new StaticPoint(
                staticPointX - _CalRecipe.ThetaOrigin.X,
                staticPointY - _CalRecipe.ThetaOrigin.Y);

            var motionPoint = CoordConvertInternal(
                translatedPoint.X, translatedPoint.Y,
                horiozntalDistance, verticalDistance,
                llMotion, hlMotion, lhMotion, hhMotion,
                _CalRecipe.CoordSystem.Clone() as CoordSystem,
                _CoordConvRecipe.CoordSystem.Clone() as CoordSystem);
            _Logger.I($"{new StaticPoint(staticPointX, staticPointY)} -> translated : {translatedPoint} -> {motionPoint}");

            return motionPoint;
        }

        private static MotionPoint CoordConvertInternal(
            double staticPointX, double staticPointY,
            double horizonntalDist, double verticalDist,
            MotionPoint llMotion, MotionPoint hlMotion, MotionPoint lhMotion, MotionPoint hhMotion,
            CoordSystem csStatic, CoordSystem csMotion) {

            return NativeCalibrator.CoordConveter(
                staticPointX, staticPointY,
                horizonntalDist, verticalDist,
                new PointData() { X = llMotion.X, Y = llMotion.Y },
                new PointData() { X = hlMotion.X, Y = hlMotion.Y },
                new PointData() { X = lhMotion.X, Y = lhMotion.Y },
                new PointData() { X = hhMotion.X, Y = hhMotion.Y },
                csStatic.RotationX.Radian, (int)csStatic.FlipY,
                csMotion.RotationX.Radian, (int)csMotion.FlipY);
        }

        /// <summary>
        /// 모션 좌표를 자재 좌표로 변환한다.
        /// </summary>
        /// <param name="motionPoint">변환하고 하는 모션 좌표</param>
        /// <param name="s1">자재 좌표 1</param>
        /// <param name="s2">자재 좌표 2/param>
        /// <param name="s3">자재 좌표 3/param>
        /// <param name="s4">자재 좌표 4/param>
        /// <param name="m1">모션 좌표 1</param>
        /// <param name="m2">모션 좌표 2</param>
        /// <param name="m3">모션 좌표 3</param>
        /// <param name="m4">모션 좌표 4</param>
        /// <returns>변환된 자재 좌표</returns>
        public static StaticPoint CoordConvertInverse(
            MotionPoint motionPoint,
            StaticPoint s1, StaticPoint s2, StaticPoint s3, StaticPoint s4,
            MotionPoint m1, MotionPoint m2, MotionPoint m3, MotionPoint m4) {

            var staticPoint = CoordConvertInverseInternal(
                motionPoint.X, motionPoint.Y,
                s1, s2, s3, s4,
                m1, m2, m3, m4);
            _StaticLogger.I($"{motionPoint} -> {staticPoint}");

            return staticPoint;
        }

        private static StaticPoint CoordConvertInverseInternal(
            double motionPointX, double motionPointY,
            StaticPoint s1, StaticPoint s2, StaticPoint s3, StaticPoint s4,
            MotionPoint m1, MotionPoint m2, MotionPoint m3, MotionPoint m4) {
            var staticPoint = NativeCalibrator.CoordConvertInverse(
                motionPointX, motionPointY,
                ToPointData(s1), ToPointData(s2), ToPointData(s3), ToPointData(s4),
                ToPointData(m1), ToPointData(m2), ToPointData(m3), ToPointData(m4));

            return staticPoint;
        }

        private static PointData ToPointData(DoublePoint p) {
            return new PointData() {
                X = p.X,
                Y = p.Y
            };
        }

        /// <summary>
        /// 자재 좌표에 얼라인 보정량을 적용한다.
        /// </summary>
        /// <param name="staticPoint">자재 좌표</param>
        /// <param name="offsetX">얼라인 X보정량</param>
        /// <param name="offsetY">얼라인 Y보정량</param>
        /// <param name="offsetT">얼라인 T보정량</param>
        /// <returns>얼라인 보정량이 적용된 자재 좌표</returns>
        public StaticPoint GetAlignedPoint(StaticPoint staticPoint, double offsetX, double offsetY, Angle offsetT) {
            // 이미지 좌표계가 아닌 경우, 시계 방향으로 회전하기 위해 회전 값을 반전(* -1)
            var rotAngle = offsetT * (int)_CalRecipe.CoordSystem.FlipY;

            // 이미지 좌표 기준 시계 방향으로 회전
            var rotatedPoint = MathUtil.Rotate(staticPoint, rotAngle, _CalRecipe.ThetaOrigin);
            var alignedPoint = new StaticPoint(rotatedPoint.X + offsetX, rotatedPoint.Y + offsetY);
            _Logger.I($"{staticPoint} -> aligned : {alignedPoint}");

            return alignedPoint;
        }

        /// <summary>
        /// 리스트로 전달된 자재 좌표에 얼라인 보정량을 적용한다.
        /// </summary>
        /// <param name="staticPoints">자재 좌표 리스트</param>
        /// <param name="offsetX">얼라인 X보정량</param>
        /// <param name="offsetY">얼라인 Y보정량</param>
        /// <param name="offsetT">얼라인 T보정량</param>
        /// <returns>얼라인 보정량이 적용된 자재 좌표 리스트</returns>
        public List<StaticPoint> GetAlignedPoints(List<StaticPoint> staticPoints, double offsetX, double offsetY, Angle offsetT) {
            List<StaticPoint> alignedStaticPoints = new List<StaticPoint>();
            foreach (var staticPoint in staticPoints) {
                alignedStaticPoints.Add(
                    GetAlignedPoint(staticPoint, offsetX, offsetY, offsetT));
            }
            return alignedStaticPoints;
        }

        /// <summary>
        /// 리스트로 전달된 자재 좌표에 얼라인 보정량을 적용하고 모션 좌표로 변환한다.
        /// </summary>
        /// <param name="staticPoints">자재 좌표 리스트</param>
        /// <param name="offsetX">얼라인 X보정량</param>
        /// <param name="offsetY">얼라인 Y보정량</param>
        /// <param name="offsetT">얼라인 T보정량</param>
        /// <param name="horiozntalDistance">패턴 간의 수평 거리</param>
        /// <param name="verticalDistance">패턴 간의 수직 거리</param>
        /// <param name="llMotion">수평 Low,  수직 Low  모션 좌표</param>
        /// <param name="hlMotion">수평 High, 수직 Low  모션 좌표</param>
        /// <param name="lhMotion">수평 Low,  수직 High 모션 좌표</param>
        /// <param name="hhMotion">수평 High, 수직 High 모션 좌표</param>
        /// <returns>얼라인 보정량이 적용된 모션 좌표 리스트</returns>
        public List<MotionPoint> CoordConvertWithAlignOffset(
            List<StaticPoint> staticPoints,
            double offsetX, double offsetY, Angle offsetT,
            double horiozntalDistance, double verticalDistance,
            MotionPoint llMotion, MotionPoint hlMotion, MotionPoint lhMotion, MotionPoint hhMotion) {
            List<MotionPoint> motionPoints = new List<MotionPoint>();

            foreach (var staticPoint in staticPoints) {
                motionPoints.Add(
                    CoordConvertWithAlignOffset(
                        staticPoint,
                        offsetX, offsetY, offsetT,
                        horiozntalDistance, verticalDistance,
                        llMotion, hlMotion, lhMotion, hhMotion));
            }

            return motionPoints;
        }

        /// <summary>
        /// 자재 좌표에 얼라인 보정량을 적용하고 모션 좌표로 변환한다.
        /// </summary>
        /// <param name="staticPoint">자재 좌표 리스트</param>
        /// <param name="offsetX">얼라인 X보정량</param>
        /// <param name="offsetY">얼라인 Y보정량</param>
        /// <param name="offsetT">얼라인 T보정량</param>
        /// <param name="horiozntalDistance">패턴 간의 수평 거리</param>
        /// <param name="verticalDistance">패턴 간의 수직 거리</param>
        /// <param name="llMotion">수평 Low,  수직 Low  모션 좌표</param>
        /// <param name="hlMotion">수평 High, 수직 Low  모션 좌표</param>
        /// <param name="lhMotion">수평 Low,  수직 High 모션 좌표</param>
        /// <param name="hhMotion">수평 High, 수직 High 모션 좌표</param>
        /// <returns>얼라인 보정량이 적용된 모션 좌표</returns>
        public MotionPoint CoordConvertWithAlignOffset(
            StaticPoint staticPoint,
            double offsetX, double offsetY, Angle offsetT,
            double horiozntalDistance, double verticalDistance,
            MotionPoint llMotion, MotionPoint hlMotion, MotionPoint lhMotion, MotionPoint hhMotion) {

            var alignedPoint = GetAlignedPoint(staticPoint, offsetX, offsetY, offsetT);
            return CoordConvert(
                alignedPoint.X, alignedPoint.Y,
                horiozntalDistance, verticalDistance,
                llMotion, hlMotion, lhMotion, hhMotion);
        }

        private void UpdateRecipe() {
            _CalRecipe = VisionRecipeManager.GetCalRecipe(_SubSysInfo);
            _CoordConvRecipe = VisionRecipeManager.GetCoordConvRecipe(_SubSysInfo);
        }
    }
}