using MwFramework.Device;
using MwFramework.Device.Motion;
using System;
using System.Diagnostics;
using System.Threading;
using System.Threading.Tasks;

namespace MainShell.Motion
{
    public sealed class MotionController
    {
        public const int DefaultTimeoutMilliseconds = 30000;
        private const int PollingIntervalMilliseconds = 20;
        private const double DefaultInPositionTolerance = 0.001d;
        private readonly SemaphoreSlim _motionLock = new SemaphoreSlim(1, 1);

        public MotionController(IAxis axis)
        {
            Axis = axis ?? throw new ArgumentNullException(nameof(axis));
            AxisFunc = axis as IAxisFunc ?? throw new ArgumentException("Axis does not support motion commands.", nameof(axis));
        }

        private IAxis Axis { get; }
        private IAxisFunc AxisFunc { get; }

        public string AxisName => Axis.Name;
        public double CurrentPos => Axis.State != null ? Axis.State.ActualPos : Axis.GetPositionImmediate();
        public bool IsBusy => Axis.State != null && Axis.State.Moving;
        public bool IsAlarm => Axis.State != null && (Axis.State.ALM || Axis.State.ServoFault || Axis.State.EMG);
        public bool InPos => Axis.State != null && Axis.State.Inpos;

        public event EventHandler<MotionStartedEventArgs> MotionStarted;
        public event EventHandler<MotionFinishedEventArgs> MotionFinished;

        public async Task<MotionResult> MoveAbsAsync(double targetPos, int timeoutMilliseconds = DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), double? positionTolerance = null)
        {
            await _motionLock.WaitAsync(cancellationToken).ConfigureAwait(false);
            try
            {
                return await ExecuteProtectedMotionAsync(
                    MotionOperation.MoveAbs,
                    targetPos,
                    timeoutMilliseconds,
                    cancellationToken,
                    prepareAction: () =>
                    {
                        EnsureMotionStateReady();
                       // EnsureWithinSoftLimit(targetPos);
                        Axis.Param.AbsPos = targetPos;
                    },
                    executeAsync: () =>
                    {
                        return Task.FromResult(Axis.AbsoluteMove());
                    },
                    positionTolerance: positionTolerance).ConfigureAwait(false);
            }
            finally
            {
                _motionLock.Release();
            }
        }

        public async Task<MotionResult> MoveRelAsync(double distance, int timeoutMilliseconds = DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), double? positionTolerance = null)
        {
            await _motionLock.WaitAsync(cancellationToken).ConfigureAwait(false);
            try
            {
                var targetPos = CurrentPos + distance;
                return await ExecuteProtectedMotionAsync(
                    MotionOperation.MoveRel,
                    targetPos,
                    timeoutMilliseconds,
                    cancellationToken,
                    prepareAction: () =>
                    {
                        EnsureMotionStateReady();
                        EnsureWithinSoftLimit(targetPos);
                        Axis.Param.IncPos = distance;
                    },
                    executeAsync: () => AxisFunc.RelativeMoveAsync(),
                    positionTolerance: positionTolerance).ConfigureAwait(false);
            }
            finally
            {
                _motionLock.Release();
            }
        }

        public async Task<MotionResult> JogAsync(Dir direction, int durationMilliseconds, int timeoutMilliseconds = DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken))
        {
            if (durationMilliseconds <= 0)
            {
                throw new ArgumentOutOfRangeException(nameof(durationMilliseconds));
            }

            await _motionLock.WaitAsync(cancellationToken).ConfigureAwait(false);
            try
            {
                return await ExecuteProtectedMotionAsync(
                    MotionOperation.Jog,
                    null,
                    timeoutMilliseconds,
                    cancellationToken,
                    prepareAction: () =>
                    {
                        EnsureMotionStateReady();
                        if (timeoutMilliseconds != Timeout.Infinite && durationMilliseconds > timeoutMilliseconds)
                        {
                            throw new TimeoutException(string.Format("Jog timeout before completion. Axis: {0}", AxisName));
                        }
                    },
                    executeAsync: async () =>
                    {
                        EnsureNoError(AxisFunc.JogStart(direction, false), MotionOperation.Jog);
                        await Task.Delay(durationMilliseconds, cancellationToken).ConfigureAwait(false);
                        return AxisFunc.StopMove();
                    }).ConfigureAwait(false);
            }
            finally
            {
                _motionLock.Release();
            }
        }

        public async Task<MotionResult> StopAsync(int timeoutMilliseconds = 3000, CancellationToken cancellationToken = default(CancellationToken))
        {
            await _motionLock.WaitAsync(cancellationToken).ConfigureAwait(false);
            try
            {
                return await ExecuteProtectedMotionAsync(
                    MotionOperation.Stop,
                    null,
                    timeoutMilliseconds,
                    cancellationToken,
                    prepareAction: null,
                    executeAsync: async () =>
                    {
                        var stopResult = AxisFunc.StopMove();
                        await Task.CompletedTask.ConfigureAwait(false);
                        return stopResult;
                    },
                    skipCommandCompletionWait: true,
                    skipInPositionValidation: true).ConfigureAwait(false);
            }
            finally
            {
                _motionLock.Release();
            }
        }

        public async Task<MotionResult> HomeAsync(int timeoutMilliseconds = DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken))
        {
            await _motionLock.WaitAsync(cancellationToken).ConfigureAwait(false);
            try
            {
                return await ExecuteProtectedMotionAsync(
                    MotionOperation.Home,
                    null,
                    timeoutMilliseconds,
                    cancellationToken,
                    prepareAction: EnsureMotionStateReady,
                    executeAsync: () => Task.Run(() => AxisFunc.Home())).ConfigureAwait(false);
            }
            finally
            {
                _motionLock.Release();
            }
        }

        private async Task<MotionResult> ExecuteProtectedMotionAsync(MotionOperation operation, double? targetPosition, int timeoutMilliseconds, CancellationToken cancellationToken, Action prepareAction, Func<Task<MotionErrorCode>> executeAsync, bool skipCommandCompletionWait = false, bool skipInPositionValidation = false, double? positionTolerance = null)
        {
            ValidateTimeout(timeoutMilliseconds);

            var startPosition = CurrentPos;
            var startedAt = DateTime.UtcNow;
            OnMotionStarted(new MotionStartedEventArgs(AxisName, operation, targetPosition, startPosition, startedAt));

            Exception failure = null;
            var cancelled = false;
            MotionResult motionResult = null;
            var watch = Stopwatch.StartNew();

            using (cancellationToken.Register(TryStop))
            {
                try
                {
                    cancellationToken.ThrowIfCancellationRequested();
                    prepareAction?.Invoke();
                    var commandTask = executeAsync != null ? executeAsync() : throw new InvalidOperationException("Motion execute delegate is null.");
                    if (!skipCommandCompletionWait)
                    {
                        var result = await WaitForCommandAsync(commandTask, watch, timeoutMilliseconds, cancellationToken).ConfigureAwait(false);
                        EnsureNoError(result, operation);
                    }
                    else
                    {
                        EnsureNoError(await commandTask.ConfigureAwait(false), operation);
                    }

                    await WaitForMotionCompletedAsync(operation, targetPosition, watch, timeoutMilliseconds, cancellationToken, skipInPositionValidation, positionTolerance).ConfigureAwait(false);
                }
                catch (OperationCanceledException ex)
                {
                    cancelled = true;
                    failure = ex;
                }
                catch (Exception ex)
                {
                    failure = ex;
                }
                finally
                {
                    var finishedAt = DateTime.UtcNow;
                    motionResult = CreateMotionResult(operation, targetPosition, startPosition, CurrentPos, failure == null && !cancelled, cancelled, failure, startedAt, finishedAt);
                    OnMotionFinished(new MotionFinishedEventArgs(motionResult));
                }
            }

            return motionResult;
        }

        private MotionResult CreateMotionResult(MotionOperation operation, double? targetPosition, double startPosition, double endPosition, bool succeeded, bool cancelled, Exception exception, DateTime startedAtUtc, DateTime finishedAtUtc)
        {
            return new MotionResult(AxisName, operation, targetPosition, startPosition, endPosition, succeeded, cancelled, exception, exception?.Message, false, startedAtUtc, finishedAtUtc);
        }

        private async Task<MotionErrorCode> WaitForCommandAsync(Task<MotionErrorCode> commandTask, Stopwatch watch, int timeoutMilliseconds, CancellationToken cancellationToken)
        {
            if (commandTask == null)
            {
                throw new InvalidOperationException("Motion command task is null.");
            }

            while (!commandTask.IsCompleted)
            {
                cancellationToken.ThrowIfCancellationRequested();
                EnsureMotionStateHealthyDuringExecution();
                ThrowIfTimeoutExceeded(timeoutMilliseconds, watch);
                await Task.Delay(PollingIntervalMilliseconds, cancellationToken).ConfigureAwait(false);
            }

            return await commandTask.ConfigureAwait(false);
        }

        private async Task WaitForMotionCompletedAsync(MotionOperation operation, double? targetPosition, Stopwatch watch, int timeoutMilliseconds, CancellationToken cancellationToken, bool skipInPositionValidation, double? positionTolerance)
        {
            Task<bool> waitPositionTask = StartWaitPositionTask(watch, timeoutMilliseconds, cancellationToken);

            while (true)
            {
                cancellationToken.ThrowIfCancellationRequested();
                EnsureMotionStateHealthyDuringExecution();
                ThrowIfTimeoutExceeded(timeoutMilliseconds, watch);

                if (waitPositionTask.IsCompleted)
                {
                    bool waitCompleted = await waitPositionTask.ConfigureAwait(false);
                    if (!waitCompleted)
                    {
                        cancellationToken.ThrowIfCancellationRequested();
                        throw new TimeoutException(string.Format("WaitPosition failed. Axis: {0}", AxisName));
                    }

                    if (IsTargetReached(operation, targetPosition, skipInPositionValidation, positionTolerance))
                    {
                        return;
                    }

                    throw new InvalidOperationException(string.Format("Axis wait completed but target position was not reached. Axis: {0}", AxisName));
                }

                await Task.Delay(PollingIntervalMilliseconds, cancellationToken).ConfigureAwait(false);
            }
        }

        private Task<bool> StartWaitPositionTask(Stopwatch watch, int timeoutMilliseconds, CancellationToken cancellationToken)
        {
            int remainingTimeoutMilliseconds = GetRemainingTimeoutMilliseconds(watch, timeoutMilliseconds);
            return Task.Run(() => AxisFunc.WaitPosition(remainingTimeoutMilliseconds, cancellationToken), cancellationToken);
        }

        private int GetRemainingTimeoutMilliseconds(Stopwatch watch, int timeoutMilliseconds)
        {
            if (timeoutMilliseconds == Timeout.Infinite)
            {
                return Timeout.Infinite;
            }

            long remainingMilliseconds = timeoutMilliseconds - watch.ElapsedMilliseconds;
            if (remainingMilliseconds <= 0)
            {
                return 1;
            }

            if (remainingMilliseconds > int.MaxValue)
            {
                return int.MaxValue;
            }

            return (int)remainingMilliseconds;
        }

        private bool IsTargetReached(MotionOperation operation, double? targetPosition, bool skipInPositionValidation, double? positionTolerance)
        {
            if (operation == MotionOperation.Stop)
            {
                return !IsBusy;
            }

            if(positionTolerance == null)
            {
                return true;
            }

            if (skipInPositionValidation || !targetPosition.HasValue)
            {
                return true;
            }

            if (InPos)
            {
                return true;
            }

            var tolerance = positionTolerance.HasValue && positionTolerance.Value > 0d ? positionTolerance.Value : DefaultInPositionTolerance;
            return Math.Abs(CurrentPos - targetPosition.Value) <= tolerance;
        }

        private void EnsureMotionStateReady()
        {
            var state = Axis.State;
            if (state == null)
            {
                throw new InvalidOperationException(string.Format("Axis state is unavailable. Axis: {0}", AxisName));
            }

            if (!state.Servo)
            {
                throw new InvalidOperationException(string.Format("Axis servo is off. Axis: {0}", AxisName));
            }

            if (state.ALM || state.ServoFault || state.EMG)
            {
                throw new InvalidOperationException(string.Format("Axis is in alarm state. Axis: {0}", AxisName));
            }

            //if (!state.CanMove)
            //{
            //    throw new InvalidOperationException(string.Format("Axis cannot move. Axis: {0}", AxisName));
            //}
        }

        private void EnsureMotionStateHealthyDuringExecution()
        {
            var state = Axis.State;
            if (state == null)
            {
                throw new InvalidOperationException(string.Format("Axis state is unavailable during motion. Axis: {0}", AxisName));
            }

            if (state.ALM || state.ServoFault || state.EMG)
            {
                throw new InvalidOperationException(string.Format("Axis entered alarm state during motion. Axis: {0}", AxisName));
            }

            if (!state.Servo)
            {
                throw new InvalidOperationException(string.Format("Axis servo turned off during motion. Axis: {0}", AxisName));
            }

            //if (!Axis.IsMotionDone())
            //{
            //    throw new InvalidOperationException(string.Format("Axis cannot continue motion. Axis: {0}", AxisName));
            //}
        }

        private void EnsureWithinSoftLimit(double targetPos)
        {
            double softMel = 0d;
            if (AxisFunc.GetSoftMel(ref softMel) == MotionErrorCode.NoError && targetPos < softMel)
            {
                throw new InvalidOperationException(string.Format("Target position {0} is below the negative soft limit {1}. Axis: {2}", targetPos, softMel, AxisName));
            }

            double softPel = 0d;
            if (AxisFunc.GetSoftPel(ref softPel) == MotionErrorCode.NoError && targetPos > softPel)
            {
                throw new InvalidOperationException(string.Format("Target position {0} is above the positive soft limit {1}. Axis: {2}", targetPos, softPel, AxisName));
            }
        }

        private static void ValidateTimeout(int timeoutMilliseconds)
        {
            if (timeoutMilliseconds <= 0 && timeoutMilliseconds != Timeout.Infinite)
            {
                throw new ArgumentOutOfRangeException(nameof(timeoutMilliseconds));
            }
        }

        private void ThrowIfTimeoutExceeded(int timeoutMilliseconds, Stopwatch watch)
        {
            if (timeoutMilliseconds != Timeout.Infinite && watch.ElapsedMilliseconds > timeoutMilliseconds)
            {
                TryStop();
                throw new TimeoutException(string.Format("Motion timeout. Axis: {0}", AxisName));
            }
        }

        private void EnsureNoError(MotionErrorCode errorCode, MotionOperation operation)
        {
            if (errorCode != MotionErrorCode.NoError)
            {
                throw new InvalidOperationException(string.Format("{0} failed on axis {1}. Error: {2}", operation, AxisName, errorCode));
            }
        }

        private void TryStop()
        {
            try
            {
                AxisFunc.StopMove();
            }
            catch
            {
            }
        }

        private void OnMotionStarted(MotionStartedEventArgs args)
        {
            MotionStarted?.Invoke(this, args);
        }

        private void OnMotionFinished(MotionFinishedEventArgs args)
        {
            MotionFinished?.Invoke(this, args);
        }
    }

    public enum MotionOperation
    {
        MoveAbs,
        MoveRel,
        Jog,
        Stop,
        Home
    }

    public sealed class MotionStartedEventArgs : EventArgs
    {
        public MotionStartedEventArgs(string axisName, MotionOperation operation, double? targetPosition, double startPosition, DateTime startedAtUtc)
        {
            AxisName = axisName;
            Operation = operation;
            TargetPosition = targetPosition;
            StartPosition = startPosition;
            StartedAtUtc = startedAtUtc;
        }

        public string AxisName { get; }
        public MotionOperation Operation { get; }
        public double? TargetPosition { get; }
        public double StartPosition { get; }
        public DateTime StartedAtUtc { get; }
    }

    public sealed class MotionFinishedEventArgs : EventArgs
    {
        public MotionFinishedEventArgs(MotionResult result)
        {
            Result = result ?? throw new ArgumentNullException(nameof(result));
        }

        public MotionResult Result { get; }
        public string AxisName => Result.AxisName;
        public MotionOperation Operation => Result.Operation;
        public double? TargetPosition => Result.TargetPosition;
        public double EndPosition => Result.EndPosition;
        public bool Succeeded => Result.Succeeded;
        public bool Cancelled => Result.Cancelled;
        public Exception Exception => Result.Exception;
        public DateTime StartedAtUtc => Result.StartedAtUtc;
        public DateTime FinishedAtUtc => Result.FinishedAtUtc;
    }
}