using MainShell.Hardware;
using MaxwellFramework.Core.Attributes;
using MwFramework.Device;
using System;
using System.Threading;
using System.Threading.Tasks;

namespace MainShell.Motion
{
    [Singleton]
    public class StagePlatformMotionService
    {
        private readonly HardwareManager _hardware;
        private readonly SafeAxisMotion _safeAxisMotion;
        private const string StagePlaneMoveSource = "StagePlatform.MovePlane";
        private static readonly string[] StagePlaneMoveTags = { "StagePlaneMove", "StagePlatform" };

        public StagePlatformMotionService(HardwareManager hardware, SafeAxisMotion safeAxisMotion)
        {
            _hardware = hardware ?? throw new ArgumentNullException(nameof(hardware));
            _safeAxisMotion = safeAxisMotion ?? throw new ArgumentNullException(nameof(safeAxisMotion));
        }

        public void MoveFlat(double targetHeight, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken))
        {
            RunSync(() => MoveFlatAsync(targetHeight, timeoutMilliseconds, cancellationToken));
        }

        public Task<MotionBatchResult> MoveFlatAsync(double targetHeight, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken))
        {
            return MovePlaneAsync(targetHeight, targetHeight, targetHeight, timeoutMilliseconds, cancellationToken);
        }

        public void MovePlane(double z7Target, double z8Target, double z9Target, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken))
        {
            RunSync(() => MovePlaneAsync(z7Target, z8Target, z9Target, timeoutMilliseconds, cancellationToken)).EnsureSuccess();
        }

        public async Task<MotionBatchResult> MovePlaneAsync(double z7Target, double z8Target, double z9Target, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken))
        {
            EnsurePlaneTargetsOrdered(z7Target, z8Target, z9Target);

            var result = await _safeAxisMotion.SafeMoveAsync(
                cancellationToken,
                MotionMoveRequest.ForAxis(GetRequiredAxis(_hardware.Axis_Stage_Z7, nameof(_hardware.Axis_Stage_Z7)), z7Target, timeoutMilliseconds, null, StagePlaneMoveSource, StagePlaneMoveTags),
                MotionMoveRequest.ForAxis(GetRequiredAxis(_hardware.Axis_Stage_Z8, nameof(_hardware.Axis_Stage_Z8)), z8Target, timeoutMilliseconds, null, StagePlaneMoveSource, StagePlaneMoveTags),
                MotionMoveRequest.ForAxis(GetRequiredAxis(_hardware.Axis_Stage_Z9, nameof(_hardware.Axis_Stage_Z9)), z9Target, timeoutMilliseconds, null, StagePlaneMoveSource, StagePlaneMoveTags)).ConfigureAwait(false);

            result.EnsureSuccess();
            return result;
        }

        private static void EnsurePlaneTargetsOrdered(double z7Target, double z8Target, double z9Target)
        {
            var spread = Math.Max(z7Target, Math.Max(z8Target, z9Target)) - Math.Min(z7Target, Math.Min(z8Target, z9Target));
            const double maxTargetSpread = 5.0d;
            if (spread > maxTargetSpread)
            {
                throw new InvalidOperationException(string.Format("Stage plane target spread {0:F3} exceeds limit {1:F3}.", spread, maxTargetSpread));
            }
        }

        private static IAxis GetRequiredAxis(IAxis axis, string axisPropertyName)
        {
            if (axis == null)
            {
                throw new InvalidOperationException(string.Format("Axis '{0}' is not available.", axisPropertyName));
            }

            return axis;
        }

        private static TResult RunSync<TResult>(Func<Task<TResult>> action)
        {
            if (action == null)
            {
                throw new ArgumentNullException(nameof(action));
            }

            return Task.Run(action).GetAwaiter().GetResult();
        }
    }
}
