/*
  * Copyright (c) 2025 LG Electronics Inc.
   * SPDX-License-Identifier: LicenseRef-LGE-Proprietary
   */
using System;
using System.Collections.Generic;
using System.Linq;
using VisionCommon;

namespace VisionSequence.Setup {
    /// <summary>
    /// 캘리브레이션 플랜
    /// </summary>
    public class CalPlan {
        private const int FIRST_STEP = 0;
        private const int SECOND_STEP = 1;
        private const int BACKLASH_STEP = 2;

        /// <summary>X, Y축 수평 이동 횟수</summary>
        public int _MoveCalStep { get; private set; }
        /// <summary>T축 회전 이동 횟수</summary>
        public int _MoveThetaStep { get; private set; }
        /// <summary>현재까지 이동 횟수</summary>
        public int _CurrentStep { get; private set; }
        /// <summary>X, Y축으로 1회 이동 거리</summary>
        public double _AxisCalLength { get; private set; }
        /// <summary>T축으로 1회 회전 각도(Degree)</summary>
        public double _ThetaCalDegree { get; private set; }

        private int _CalXaxisStep { get; set; }
        private int _CalYaxisStep { get; set; }
        private int _CalTaxisStep { get; set; }
        private Dictionary<string, object> Plan = new Dictionary<string, object>();

        /// <summary>
        /// 생성자
        /// </summary>
        /// <param name="moveStep">X, Y축 수평 이동 횟수</param>
        /// <param name="axisLength">X, Y축으로 1회 이동 거리</param>
        /// <param name="thetaStep">T축 회전 이동 횟수</param>
        /// <param name="thetaDegree">T축으로 1회 회전 각도(Degree)</param>
        public CalPlan(int moveStep, double axisLength, int thetaStep, double thetaDegree) {
            _MoveCalStep = moveStep;
            _MoveThetaStep = thetaStep;
            _AxisCalLength = axisLength;
            _ThetaCalDegree = thetaDegree;
            _CalXaxisStep = moveStep + BACKLASH_STEP;
            _CalYaxisStep = moveStep + BACKLASH_STEP;
            _CalTaxisStep = thetaStep + BACKLASH_STEP;
            _CurrentStep = 1;
        }

        private void MakeXaxisCalPlan(string axis, string actionType) {
            if (_CalXaxisStep == BACKLASH_STEP) {
                return;
            }
            else {
                MakeAxisCalPlan(_CalXaxisStep, _AxisCalLength, axis, actionType);
            }
        }

        private void MakeYaxisCalPlan(string axis, string actionType) {
            if (_CalYaxisStep == BACKLASH_STEP) {
                return;
            }
            else {
                MakeAxisCalPlan(_CalYaxisStep, _AxisCalLength, axis, actionType);
            }
        }

        private void MakeTaxisCalPlan(string axis, string actionType) {
            if (_CalTaxisStep == BACKLASH_STEP) {
                return;
            }

            MakeAxisCalPlan(_CalTaxisStep, _ThetaCalDegree, axis, actionType);
        }

        private void MakeAxisCalPlan(int calAxisStep, double calAxisLength, string moveAxis, string actionType) {
            string MoveAxis = moveAxis.Remove(1);
            double CalAxisLength = calAxisLength;
            double BackLash = -calAxisLength;

            try {
                for (int i = 0; i <= calAxisStep; i++) {
                    Dictionary<string, object> CalDistance = new Dictionary<string, object>();
                    Dictionary<string, object> CalMove = new Dictionary<string, object>();
                    Dictionary<string, object> CalAction = new Dictionary<string, object>();
                    Dictionary<string, object> CalAxis = new Dictionary<string, object>();
                    if (i == FIRST_STEP) {
                        if (MoveAxis == Constants.X) {
                            double startPosition = CalculateStartPosition(_MoveCalStep, CalAxisLength) + BackLash;
                            CalDistance.Add(Constants.X, startPosition.ToString());
                            CalDistance.Add(Constants.Y, "0");
                            CalDistance.Add(Constants.T, "0");
                        }
                        else if (MoveAxis == Constants.Y) {
                            double startPosition = CalculateStartPosition(_MoveCalStep, CalAxisLength) + BackLash;
                            CalDistance.Add(Constants.X, "0");
                            CalDistance.Add(Constants.Y, startPosition.ToString());
                            CalDistance.Add(Constants.T, "0");
                        }
                        else if (MoveAxis == Constants.T) {
                            double startPosition = CalculateStartPosition(_MoveThetaStep, CalAxisLength) + BackLash;
                            CalDistance.Add(Constants.X, "0");
                            CalDistance.Add(Constants.Y, "0");
                            CalDistance.Add(Constants.T, startPosition.ToString());
                        }
                        else {
                            Console.WriteLine("Axis Error");
                        }
                        CalMove.Add(Constants.BACKLASH, CalDistance);
                        CalAxis.Add(moveAxis, CalMove);
                        CalAction.Add(actionType, CalAxis);
                        Plan.Add(_CurrentStep.ToString(), CalAction);
                    }
                    else if (i == SECOND_STEP) {
                        if (MoveAxis == Constants.X) {
                            CalDistance.Add(Constants.X, (-BackLash).ToString());
                            CalDistance.Add(Constants.Y, "0");
                            CalDistance.Add(Constants.T, "0");
                        }
                        else if (MoveAxis == Constants.Y) {
                            CalDistance.Add(Constants.X, "0");
                            CalDistance.Add(Constants.Y, (-BackLash).ToString());
                            CalDistance.Add(Constants.T, "0");
                        }
                        else if (MoveAxis == Constants.T) {
                            CalDistance.Add(Constants.X, "0");
                            CalDistance.Add(Constants.Y, "0");
                            CalDistance.Add(Constants.T, (-BackLash).ToString());
                        }
                        else {
                            Console.WriteLine("Axis Error");
                        }
                        CalMove.Add(Constants.MOVE, CalDistance);
                        CalAxis.Add(moveAxis, CalMove);
                        CalAction.Add(actionType, CalAxis);
                        Plan.Add(_CurrentStep.ToString(), CalAction);
                    }
                    else if (i == calAxisStep) {
                        if (MoveAxis == Constants.X) {
                            double measurePosition = CalculateMeasurePosition(_MoveCalStep, CalAxisLength);
                            CalDistance.Add(Constants.X, measurePosition.ToString());
                            CalDistance.Add(Constants.Y, "0");
                            CalDistance.Add(Constants.T, "0");
                        }
                        else if (MoveAxis == Constants.Y) {
                            double measurePosition = CalculateMeasurePosition(_MoveCalStep, CalAxisLength);
                            CalDistance.Add(Constants.X, "0");
                            CalDistance.Add(Constants.Y, measurePosition.ToString());
                            CalDistance.Add(Constants.T, "0");
                        }
                        else if (MoveAxis == Constants.T) {
                            double measurePosition = CalculateMeasurePosition(_MoveThetaStep, CalAxisLength);
                            CalDistance.Add(Constants.X, "0");
                            CalDistance.Add(Constants.Y, "0");
                            CalDistance.Add(Constants.T, measurePosition.ToString());
                        }
                        else {
                            Console.WriteLine("Axis Error");
                        }
                        CalMove.Add(Constants.CALCULATION, CalDistance);
                        CalAxis.Add(moveAxis, CalMove);
                        CalAction.Add(actionType, CalAxis);
                        Plan.Add(_CurrentStep.ToString(), CalAction);
                    }
                    else {
                        if (MoveAxis == Constants.X) {
                            CalDistance.Add(Constants.X, CalAxisLength.ToString());
                            CalDistance.Add(Constants.Y, "0");
                            CalDistance.Add(Constants.T, "0");
                        }
                        else if (MoveAxis == Constants.Y) {
                            CalDistance.Add(Constants.X, "0");
                            CalDistance.Add(Constants.Y, CalAxisLength.ToString());
                            CalDistance.Add(Constants.T, "0");
                        }
                        else if (MoveAxis == Constants.T) {
                            CalDistance.Add(Constants.X, "0");
                            CalDistance.Add(Constants.Y, "0");
                            CalDistance.Add(Constants.T, CalAxisLength.ToString());
                        }
                        else {
                            Console.WriteLine("Axis Error");
                        }

                        CalMove.Add(Constants.MOVE, CalDistance);
                        CalAxis.Add(moveAxis, CalMove);
                        CalAction.Add(actionType, CalAxis);
                        Plan.Add(_CurrentStep.ToString(), CalAction);
                    }
                    _CurrentStep++;
                }
            }
            catch (Exception e) {
                Console.WriteLine(e.ToString());
            }
        }

        private double CalculateStartPosition(int calAxisStep, double calAxisLength) {
            int stepsToStartPos = (calAxisStep + 1) / 2;
            int directionToStartPos = -1;

            return (calAxisLength * stepsToStartPos * directionToStartPos);
        }

        private double CalculateMeasurePosition(int calAxisStep, double calAxisLength) {
            int stepsToMeasurePos = calAxisStep / 2;
            return (stepsToMeasurePos * calAxisLength * (-1));
        }

        /// <summary>
        /// 캘리브레이션 플랜을 생성한다.
        /// </summary>
        /// <param name="isCalPixelExist">픽셀 사이즈 측정 플래그</param>
        /// <param name="isX">X축 캘리브레이션 사용 플래그</param>
        /// <param name="isY">Y축 캘리브레이션 사용 플래그</param>
        /// <param name="isT">T축 캘리브레이션 사용 플래그</param>
        /// <returns></returns>
        public Dictionary<string, object> MakeCalPlan(bool isCalPixelExist, bool isX, bool isY, bool isT) {
            if (isCalPixelExist) {
                if (isX) {
                    MakeXaxisCalPlan(Constants.X_PLUS, Constants.X_CAL);
                }
                if (isY) {
                    MakeYaxisCalPlan(Constants.Y_PLUS, Constants.Y_CAL);
                }
            }
            if (isT) {
                MakeTaxisCalPlan(Constants.T_PLUS, Constants.THETA_CAL);
            }

            return Plan;
        }
    }
}