using MainShell.Common;
using MainShell.Hardware;
using MainShell.Log;
using MaxwellFramework.Core.Attributes;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;
using System.Windows;

namespace MainShell.Motion
{
    /// <summary>
    /// 逼近对位服务：实现迭代式的多轴对齐算法
    /// </summary>
  
    public class ApproachAlignmentService
    {
        private readonly SafeAxisMotion _safeAxisMotion;
        private readonly ICenterRecognizer _centerRecognizer;
        private readonly ICoordinateTransformer _coordinateTransformer;
        private readonly HardwareManager _hardwareManager;

        public ApproachAlignmentService(
            SafeAxisMotion safeAxisMotion,
            ICenterRecognizer centerRecognizer,
            ICoordinateTransformer coordinateTransformer,
            HardwareManager hardwareManager)
        {
            _safeAxisMotion = safeAxisMotion ?? throw new ArgumentNullException(nameof(safeAxisMotion));
            _centerRecognizer = centerRecognizer ?? throw new ArgumentNullException(nameof(centerRecognizer));
            _coordinateTransformer = coordinateTransformer ?? throw new ArgumentNullException(nameof(coordinateTransformer));
            _hardwareManager = hardwareManager ?? throw new ArgumentNullException(nameof(hardwareManager));
        }

        /// <summary>
        /// 执行逼近对位
        /// </summary>
        /// <param name="request">逼近请求参数</param>
        /// <param name="cancellationToken">取消令牌</param>
        /// <returns>逼近结果</returns>
        public async Task<ApproachAlignmentResult> ApproachAlignmentAsync(
            ApproachAlignmentRequest request,
            CancellationToken cancellationToken = default(CancellationToken))
        {
            if (request == null)
            {
                throw new ArgumentNullException(nameof(request));
            }

            ApproachAlignmentResult result = new ApproachAlignmentResult();

            try
            {
                (double CenterX, double CenterY)? center = null;

                // 循环执行逼近
                for (int iteration = 0; iteration < request.MaxIterations; iteration++)
                {
                    cancellationToken.ThrowIfCancellationRequested();

                    $"Approach alignment iteration {iteration + 1}/{request.MaxIterations} started.".LogInfo();

                    // 第一步：识别中心。首次迭代执行识别，后续迭代复用上一次移动后的识别结果。
                    if (center == null)
                    {
                        center = await RecognizeCenterAsync(
                            request.Camera,
                            request.RecognitionTimeoutMilliseconds,
                            request.RecognitionParameters,
                            cancellationToken).ConfigureAwait(false);
                        if (center == null)
                        {
                            throw new InvalidOperationException("Center recognition failed.");
                        }

                        $"Center recognized: X={center.Value.CenterX:F4}, Y={center.Value.CenterY:F4}".LogInfo();
                    }
                    else
                    {
                        $"Center reused from previous iteration: X={center.Value.CenterX:F4}, Y={center.Value.CenterY:F4}".LogInfo();
                    }

                    // 第二步：坐标转换
                    Point rulerPoint = BuildRulerPoint(request.Axes);
                    Point pixelPoint = new Point(center.Value.CenterX, center.Value.CenterY);
                    string cameraName = GetCameraName(request.Camera);

                    CoordinateTransformResult transformResult = await _coordinateTransformer.TransformAsync(
                        rulerPoint,
                        pixelPoint,
                        cameraName).ConfigureAwait(false);

                    if (!transformResult.Succeeded)
                    {
                        throw new InvalidOperationException($"Coordinate transformation failed: {transformResult.Message}");
                    }

                    $"Coordinate transformation completed: Target=({transformResult.TargetPoint.X:F4}, {transformResult.TargetPoint.Y:F4}).".LogInfo();

                    // 第三步：移动所有轴
                    MotionMoveRequest[] moveRequests = CreateMoveRequests(
                        request.Axes,
                        transformResult.TargetPoint,
                        request.MoveTimeoutMilliseconds);

                    MotionBatchResult moveResult = await _safeAxisMotion.SafeMoveAsync(cancellationToken, moveRequests).ConfigureAwait(false);
                    moveResult.EnsureSuccess();

                    $"Axes moved successfully: {moveResult.Results.Count} axes completed.".LogInfo();

                    // 第四步：再次识别中心
                    (double CenterX, double CenterY)? newCenter = await RecognizeCenterAsync(
                        request.Camera,
                        request.RecognitionTimeoutMilliseconds,
                        request.RecognitionParameters,
                        cancellationToken).ConfigureAwait(false);
                    if (newCenter == null)
                    {
                        throw new InvalidOperationException("Center recognition failed on second attempt.");
                    }

                    $"Center recognized again: X={newCenter.Value.CenterX:F4}, Y={newCenter.Value.CenterY:F4}".LogInfo();

                    // 第五步：转换新中心的轴位置
                    Point newRulerPoint = BuildRulerPoint(request.Axes);
                    Point newPixelPoint = new Point(newCenter.Value.CenterX, newCenter.Value.CenterY);
                    CoordinateTransformResult newTransform = await _coordinateTransformer.TransformAsync(
                        newRulerPoint,
                        newPixelPoint,
                        cameraName).ConfigureAwait(false);

                    if (!newTransform.Succeeded)
                    {
                        throw new InvalidOperationException($"Coordinate transformation failed on second attempt: {newTransform.Message}");
                    }

                    // 第六步：检查误差
                    Dictionary<string, double> errors = new Dictionary<string, double>();
                    bool allWithinTolerance = true;

                    foreach (ApproachAlignmentAxis axis in request.Axes)
                    {
                        double currentAxisPos = GetCurrentAxisPosition(axis.AxisName);
                        double expectedAxisPos = GetExpectedAxisPosition(axis.AxisName, newTransform.TargetPoint);
                        double error = Math.Abs(currentAxisPos - expectedAxisPos);

                        errors[axis.AxisName] = error;
                        result.FinalAxisPositions[axis.AxisName] = currentAxisPos;

                        if (error > axis.ToleranceValue)
                        {
                            allWithinTolerance = false;
                        }

                        $"Axis: {axis.AxisName}, Current: {currentAxisPos:F4}, Expected: {expectedAxisPos:F4}, Error: {error:F6}, Tolerance: {axis.ToleranceValue:F4}, WithinTolerance: {error <= axis.ToleranceValue}".LogInfo();
                    }

                    result.FinalErrors.Clear();
                    foreach (KeyValuePair<string, double> error in errors)
                    {
                        result.FinalErrors.Add(error.Key, error.Value);
                    }

                    result.CompletedIterations = iteration + 1;

                    if (allWithinTolerance)
                    {
                        result.Succeeded = true;
                        $"Approach alignment succeeded after {result.CompletedIterations} iteration(s).".LogInfo();
                        break;
                    }

                    center = newCenter;
                    $"Approach alignment iteration {iteration + 1} completed. Not all axes within tolerance, continuing...".LogInfo();
                }

                if (!result.Succeeded)
                {
                    $"Approach alignment failed after maximum iterations ({request.MaxIterations}).".LogInfo();
                }
            }
            catch (OperationCanceledException)
            {
                result.Exception = new OperationCanceledException("Approach alignment was cancelled.");
                "Approach alignment cancelled.".LogInfo();
            }
            catch (Exception ex)
            {
                result.Exception = ex;
                $"Approach alignment error: {ex.Message}".LogSysError();
            }

            return result;
        }

        public async Task<ApproachAlignmentResult> SingleAlignmentAsync(
            ApproachAlignmentRequest request,
            CancellationToken cancellationToken = default(CancellationToken))
        {
            if (request == null)
            {
                throw new ArgumentNullException(nameof(request));
            }

            ApproachAlignmentResult result = new ApproachAlignmentResult();

            try
            {
                cancellationToken.ThrowIfCancellationRequested();
                "Single alignment started.".LogInfo();

                (double CenterX, double CenterY)? center = await RecognizeCenterAsync(
                    request.Camera,
                    request.RecognitionTimeoutMilliseconds,
                    request.RecognitionParameters,
                    cancellationToken).ConfigureAwait(false);
                if (center == null)
                {
                    throw new InvalidOperationException("Center recognition failed.");
                }

                Point rulerPoint = BuildRulerPoint(request.Axes);
                Point pixelPoint = new Point(center.Value.CenterX, center.Value.CenterY);
                string cameraName = GetCameraName(request.Camera);

                CoordinateTransformResult transformResult = await _coordinateTransformer.TransformAsync(
                    rulerPoint,
                    pixelPoint,
                    cameraName).ConfigureAwait(false);
                if (!transformResult.Succeeded)
                {
                    throw new InvalidOperationException($"Coordinate transformation failed: {transformResult.Message}");
                }

                MotionMoveRequest[] moveRequests = CreateMoveRequests(
                    request.Axes,
                    transformResult.TargetPoint,
                    request.MoveTimeoutMilliseconds);

                MotionBatchResult moveResult = await _safeAxisMotion.SafeMoveAsync(cancellationToken, moveRequests).ConfigureAwait(false);
                moveResult.EnsureSuccess();

                result.Succeeded = ProcessAlignmentResult(request.Axes, transformResult.TargetPoint, result);
                result.CompletedIterations = 1;

                if (result.Succeeded)
                {
                    "Single alignment completed successfully.".LogInfo();
                }
                else
                {
                    result.Exception = new InvalidOperationException("Single alignment completed but result exceeded tolerance.");
                    "Single alignment completed but result exceeded tolerance.".LogInfo();
                }
            }
            catch (OperationCanceledException)
            {
                result.Exception = new OperationCanceledException("Single alignment was cancelled.");
                "Single alignment cancelled.".LogInfo();
            }
            catch (Exception ex)
            {
                result.Exception = ex;
                $"Single alignment error: {ex.Message}".LogSysError();
            }

            return result;
        }

        private async Task<(double CenterX, double CenterY)?> RecognizeCenterAsync(
            CameraType camera,
            int timeoutMilliseconds,
            CenterRecognitionParameters parameters,
            CancellationToken cancellationToken)
        {
            try
            {
                return await _centerRecognizer.RecognizeCenterAsync(
                    camera,
                    timeoutMilliseconds,
                    parameters,
                    cancellationToken).ConfigureAwait(false);
            }
            catch (Exception ex)
            {
                $"Center recognition exception: {ex.Message}".LogSysError();
                throw;
            }
        }

        private double GetCurrentAxisPosition(string axisName)
        {
            try
            {
                MwFramework.Device.IAxis axis = _hardwareManager.GetAxisByName(axisName);
                if (axis == null)
                {
                    throw new InvalidOperationException($"Axis '{axisName}' not found.");
                }

                return axis.State != null ? axis.State.ActualPos : axis.GetPositionImmediate();
            }
            catch (Exception ex)
            {
                $"Failed to get current position for axis '{axisName}': {ex.Message}".LogSysError();
                throw;
            }
        }

        private MotionMoveRequest[] CreateMoveRequests(
            IReadOnlyList<ApproachAlignmentAxis> axes,
            Point targetPoint,
            int moveTimeoutMilliseconds)
        {
            List<MotionMoveRequest> moveRequests = new List<MotionMoveRequest>();

            foreach (ApproachAlignmentAxis axis in axes)
            {
                double targetPosition = GetExpectedAxisPosition(axis.AxisName, targetPoint);
                moveRequests.Add(MotionMoveRequest.ForAxisName(axis.AxisName, targetPosition, moveTimeoutMilliseconds));
            }

            return moveRequests.ToArray();
        }

        private bool ProcessAlignmentResult(
            IReadOnlyList<ApproachAlignmentAxis> axes,
            Point targetPoint,
            ApproachAlignmentResult result)
        {
            bool allWithinTolerance = true;

            result.FinalErrors.Clear();

            foreach (ApproachAlignmentAxis axis in axes)
            {
                double currentAxisPos = GetCurrentAxisPosition(axis.AxisName);
                double expectedAxisPos = GetExpectedAxisPosition(axis.AxisName, targetPoint);
                double error = Math.Abs(currentAxisPos - expectedAxisPos);

                result.FinalErrors[axis.AxisName] = error;
                result.FinalAxisPositions[axis.AxisName] = currentAxisPos;

                if (error > axis.ToleranceValue)
                {
                    allWithinTolerance = false;
                }

                $"Axis: {axis.AxisName}, Current: {currentAxisPos:F4}, Expected: {expectedAxisPos:F4}, Error: {error:F6}, Tolerance: {axis.ToleranceValue:F4}, WithinTolerance: {error <= axis.ToleranceValue}".LogInfo();
            }

            return allWithinTolerance;
        }

        private Point BuildRulerPoint(IReadOnlyList<ApproachAlignmentAxis> axes)
        {
            double x = 0;
            double y = 0;
            bool hasX = false;
            bool hasY = false;

            foreach (ApproachAlignmentAxis axis in axes)
            {
                if (axis == null || string.IsNullOrWhiteSpace(axis.AxisName))
                {
                    continue;
                }

                double currentPosition = GetCurrentAxisPosition(axis.AxisName);
                if (IsYAxis(axis.AxisName))
                {
                    if (!hasY)
                    {
                        y = currentPosition;
                        hasY = true;
                    }
                }
                else
                {
                    if (!hasX)
                    {
                        x = currentPosition;
                        hasX = true;
                    }
                }
            }

            return new Point(x, y);
        }

        private double GetExpectedAxisPosition(string axisName, Point targetPoint)
        {
            return IsYAxis(axisName) ? targetPoint.Y : targetPoint.X;
        }

        private static bool IsYAxis(string axisName)
        {
            return !string.IsNullOrWhiteSpace(axisName) && axisName.IndexOf("Y", StringComparison.OrdinalIgnoreCase) >= 0;
        }

        private string GetCameraName(CameraType camera)
        {
           return _hardwareManager.GetCameraName(camera);
        }
    }
}