using MainShell.Common;
using MainShell.Log;
using MainShell.Motion;
using MainShell.ProcessResult;
using MainShell.Recipe.Models;
using MainShell.Recipe.Models.SubstrateParameter;
using MW.WorkFlow;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading.Tasks;
using System.Windows;

namespace MainShell.Process
{
    public class SubstratePositionMotionService
    {
        private readonly ApproachAlignmentService _approachAlignmentService;

        public SubstratePositionMotionService(ApproachAlignmentService approachAlignmentService)
        {
            _approachAlignmentService = approachAlignmentService ?? throw new ArgumentNullException(nameof(approachAlignmentService));
        }

        public async Task ExecuteAsync(WorkflowContext context, ActivityControl activityControl)
        {
            if (context == null)
            {
                throw new ArgumentNullException(nameof(context));
            }

            if (activityControl == null)
            {
                throw new ArgumentNullException(nameof(activityControl));
            }

            RecipeManager recipeManager = GetRecipeManager(context);
            ProcessResultManager processResultManager = GetProcessResultManager(context);
            SubstrateRecipe substrateRecipe = GetCurrentRecipe(recipeManager);
            List<MarkData> enabledMarks = GetEnabledMarks(substrateRecipe);
            SubstratePositionProcessResult processResult = InitializeProcessResult(processResultManager, enabledMarks.Count);

            try
            {
                LogManager.LogInfo($"SubstratePosition: start mark positioning. Total enabled marks: {enabledMarks.Count}");

                for (int i = 0; i < enabledMarks.Count; i++)
                {
                    MarkData markData = enabledMarks[i];
                    string markName = string.IsNullOrWhiteSpace(markData?.TemplateName) ? $"Mark_{i}" : markData.TemplateName;

                    await CheckCancellationAndPauseAsync(activityControl).ConfigureAwait(false);

                    LogManager.LogInfo($"SubstratePosition: executing approach alignment for mark[{i}] '{markName}'");
                    ApproachAlignmentResult alignmentResult = await ExecuteApproachAlignmentAsync(markData, activityControl).ConfigureAwait(false);

                    if (!alignmentResult.Succeeded)
                    {
                        string reason = alignmentResult.Message ?? string.Empty;
                        string errorMsg = $"SubstratePosition: approach alignment failed for mark[{i}] '{markName}'. Reason: {reason}";
                        LogManager.LogSysError(errorMsg);
                        throw new LocalizedProcessException(
                            MessageKey.ProcessSubstratePositionAlignmentFailedWithReason,
                            new[] { markName, reason });
                    }

                    Point recognizedPos = ExtractRecognizedPosition(alignmentResult, markData);
                    processResult.MarkResults.Add(recognizedPos);
                    processResult.RecognizedMarkCount++;

                    LogManager.LogInfo(
                        string.Format(
                            "SubstratePosition: mark[{0}] aligned successfully. Iterations: {1}. Position: ({2:F4}, {3:F4})",
                            i,
                            alignmentResult.CompletedIterations,
                            recognizedPos.X,
                            recognizedPos.Y));
                }

                processResult.SubstrateAngle = CalculateSubstrateAngle(processResult.MarkResults);
                processResult.IsSuccess = true;
                processResult.ErrorMessage = null;
                processResult.ErrorMessageKey = MessageKey.None;
                processResult.ErrorMessageArguments = Array.Empty<string>();

                LogManager.LogInfo(
                    string.Format(
                        "SubstratePosition: completed successfully. Recognized: {0}/{1}, Angle: {2:F6}",
                        processResult.RecognizedMarkCount,
                        processResult.AvailableMarkCount,
                        processResult.SubstrateAngle));
            }
            catch (OperationCanceledException)
            {
                processResult.IsSuccess = false;
                processResult.ErrorMessage = null;
                processResult.ErrorMessageKey = MessageKey.None;
                processResult.ErrorMessageArguments = Array.Empty<string>();
                LogManager.LogInfo("SubstratePosition: operation canceled.");
                throw;
            }
            catch (LocalizedProcessException ex)
            {
                processResult.IsSuccess = false;
                processResult.ErrorMessageKey = ex.FailureMessageKey;
                processResult.ErrorMessageArguments = ex.FailureMessageArguments;
                processResult.ErrorMessage = ex.Message;
                LogManager.LogSysError(ex.Message);
            }
            catch (Exception ex)
            {
                processResult.IsSuccess = false;
                processResult.ErrorMessageKey = MessageKey.ProcessStepFailedWithReason;
                processResult.ErrorMessageArguments = new[] { ProcessFlowName.SubstratePositionFlow, ex.Message ?? string.Empty };
                processResult.ErrorMessage = LanguageResourceHelper.Format(
                    processResult.ErrorMessageKey,
                    LocalizedProcessException.ConvertArguments(processResult.ErrorMessageArguments));
                LogManager.LogSysError(processResult.ErrorMessage);
            }
            finally
            {
                WriteResultToContext(context, processResultManager, processResult);
            }
        }

        private async Task<ApproachAlignmentResult> ExecuteApproachAlignmentAsync(MarkData markData, ActivityControl activityControl)
        {
            if (markData == null)
            {
                throw new ArgumentNullException(nameof(markData));
            }

            List<ApproachAlignmentAxis> axes = new List<ApproachAlignmentAxis>
            {
                new ApproachAlignmentAxis("X", 0.1),
                new ApproachAlignmentAxis("Y", 0.1)
            };

            ApproachAlignmentRequest request = new ApproachAlignmentRequest(axes)
            {
                MaxIterations = 5,
                MoveTimeoutMilliseconds = 30000,
                RecognitionTimeoutMilliseconds = 10000
            };

            return await _approachAlignmentService
                .ApproachAlignmentAsync(request, activityControl.CancellationToken)
                .ConfigureAwait(false);
        }

        private static Point ExtractRecognizedPosition(ApproachAlignmentResult alignmentResult, MarkData markData)
        {
            if (alignmentResult == null)
            {
                throw new ArgumentNullException(nameof(alignmentResult));
            }

            if (alignmentResult.FinalAxisPositions == null ||
                !alignmentResult.FinalAxisPositions.TryGetValue("X", out var x) ||
                !alignmentResult.FinalAxisPositions.TryGetValue("Y", out var y))
            {
                throw new LocalizedProcessException(
                    MessageKey.ProcessSubstratePositionAlignmentFailedWithReason,
                    new[]
                    {
                        string.IsNullOrWhiteSpace(markData?.TemplateName) ? string.Empty : markData.TemplateName,
                        "Recognized position is missing X or Y axis data."
                    });
            }

            return new Point(x, y);
        }

        private static async Task CheckCancellationAndPauseAsync(ActivityControl activityControl)
        {
            activityControl.ThrowIfCancellationRequested();
            await activityControl.CheckPauseAsync().ConfigureAwait(false);
        }

        private static RecipeManager GetRecipeManager(WorkflowContext context)
        {
            return context.GetData<RecipeManager>(WorkflowContextKeys.RecipeManager);
        }

        private static ProcessResultManager GetProcessResultManager(WorkflowContext context)
        {
            return context.GetData<ProcessResultManager>(WorkflowContextKeys.ProcessResultManager);
        }

        private static SubstrateRecipe GetCurrentRecipe(RecipeManager recipeManager)
        {
            if (recipeManager == null)
            {
                throw new ArgumentNullException(nameof(recipeManager));
            }

            if (recipeManager.CurrentSubstrateRecipe == null)
            {
                throw new LocalizedProcessException(
                    MessageKey.ProcessSubstratePositionRecipeNotLoaded,
                    Array.Empty<string>());
            }

            return recipeManager.CurrentSubstrateRecipe;
        }

        private static List<MarkData> GetEnabledMarks(SubstrateRecipe substrateRecipe)
        {
            if (substrateRecipe?.SubtrateMarkParameterInfo?.MarkDatas == null)
            {
                throw new LocalizedProcessException(
                    MessageKey.ProcessSubstratePositionMarkParameterMissing,
                    Array.Empty<string>());
            }

            List<MarkData> enabledMarks = substrateRecipe.SubtrateMarkParameterInfo.MarkDatas
                .Where(mark => mark != null && mark.IsEnabled)
                .ToList();

            if (enabledMarks.Count == 0)
            {
                throw new LocalizedProcessException(
                    MessageKey.ProcessSubstratePositionNoEnabledMarks,
                    Array.Empty<string>());
            }

            return enabledMarks;
        }

        private static SubstratePositionProcessResult InitializeProcessResult(ProcessResultManager processResultManager, int availableMarkCount)
        {
            if (processResultManager == null)
            {
                throw new ArgumentNullException(nameof(processResultManager));
            }

            SubstratePositionProcessResult processResult = processResultManager.SubstratePositionResult;
            processResult.IsSuccess = false;
            processResult.AvailableMarkCount = availableMarkCount;
            processResult.RecognizedMarkCount = 0;
            processResult.ErrorMessage = null;
            processResult.ErrorMessageKey = MessageKey.None;
            processResult.ErrorMessageArguments = Array.Empty<string>();
            processResult.SubstrateAngle = 0;
            processResult.MarkResults.Clear();
            return processResult;
        }

        private static double CalculateSubstrateAngle(List<Point> markResults)
        {
            if (markResults == null || markResults.Count < 2)
            {
                return 0;
            }

            double centroidX = markResults.Average(point => point.X);
            double centroidY = markResults.Average(point => point.Y);

            double sumXX = 0;
            double sumYY = 0;
            double sumXY = 0;

            foreach (Point point in markResults)
            {
                double dx = point.X - centroidX;
                double dy = point.Y - centroidY;
                sumXX += dx * dx;
                sumYY += dy * dy;
                sumXY += dx * dy;
            }

            if (Math.Abs(sumXX) < 1e-9 && Math.Abs(sumYY) < 1e-9 && Math.Abs(sumXY) < 1e-9)
            {
                return 0;
            }

            double angleRadians = 0.5 * Math.Atan2(2 * sumXY, sumXX - sumYY);
            return angleRadians * 180.0 / Math.PI;
        }

        private static void WriteResultToContext(
            WorkflowContext context,
            ProcessResultManager processResultManager,
            SubstratePositionProcessResult processResult)
        {
            context.SetData(WorkflowContextKeys.SubstratePositionResult, processResult);
            processResultManager.SaveSubstratePositionResult();
        }
    }
}