using MaxwellFramework.Core.Attributes;
using MainShell.Hardware;
using MainShell.Log;
using MainShell.Motion.Safety;
using MwFramework.Device;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;

namespace MainShell.Motion
{
    public class SafeAxisMotion
    {
        private sealed class ResolvedMotionMoveRequest
        {
            public ResolvedMotionMoveRequest(MotionMoveRequest request, IAxis axis)
            {
                Request = request ?? throw new ArgumentNullException(nameof(request));
                Axis = axis ?? throw new ArgumentNullException(nameof(axis));
            }

            public MotionMoveRequest Request { get; private set; }
            public IAxis Axis { get; private set; }
        }

        private readonly HardwareManager _hardware;
        private readonly MotionControllerRegistry _controllerRegistry;
        private readonly MotionPrecheckService _motionPrecheckService;
        private readonly MotionAlarmReporter _motionAlarmReporter;

        public SafeAxisMotion(HardwareManager hardware, MotionControllerRegistry controllerRegistry, MotionPrecheckService motionPrecheckService, MotionAlarmReporter motionAlarmReporter)
        {
            _hardware = hardware ?? throw new ArgumentNullException(nameof(hardware));
            _controllerRegistry = controllerRegistry ?? throw new ArgumentNullException(nameof(controllerRegistry));
            _motionPrecheckService = motionPrecheckService ?? throw new ArgumentNullException(nameof(motionPrecheckService));
            _motionAlarmReporter = motionAlarmReporter ?? throw new ArgumentNullException(nameof(motionAlarmReporter));
        }

        #region ??????
        public void SafeMove(params MotionMoveRequest[] requests)
        {
            SafeMove(CancellationToken.None, requests);
        }

        public void SafeMove(CancellationToken cancellationToken, params MotionMoveRequest[] requests)
        {
            RunSync(() => SafeMoveAsync(cancellationToken, requests)).EnsureSuccess();
        }

        public Task<MotionBatchResult> SafeMoveAsync(params MotionMoveRequest[] requests)
        {
            return SafeMoveAsync(CancellationToken.None, requests);
        }

        public async Task<MotionBatchResult> SafeMoveAsync(CancellationToken cancellationToken, params MotionMoveRequest[] requests)
        {
            if (requests == null || requests.Length == 0)
            {
                return new MotionBatchResult(Array.Empty<MotionResult>());
            }

            var batchId = CreateBatchId(requests);
            var resolvedRequests = requests.Select(x => ResolveRequest(EnsureRequestBatchMetadata(x, batchId))).ToArray();
            var axes = resolvedRequests.Select(x => x.Axis).ToArray();
            await _motionPrecheckService.ValidateAsync(CreateSafetyContext(MotionRequestKind.MoveAbs, resolvedRequests), cancellationToken, requests.Select(x => x.AlarmId).FirstOrDefault(x => x.HasValue)).ConfigureAwait(false);
            using (var linkedCancellationTokenSource = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken))
            {
                var tasks = resolvedRequests.Select(x => MoveAbsInternalAsync(x, linkedCancellationTokenSource.Token, linkedCancellationTokenSource, axes)).ToArray();
                var results = await Task.WhenAll(tasks).ConfigureAwait(false);
                return new MotionBatchResult(results);
            }
        }

        public void MoveAbs(string axisName, double targetPos)
        {
            SafeMove(MotionMoveRequest.ForAxisName(axisName, targetPos));
        }

        public void MoveAbs(string axisName, double targetPos, CancellationToken cancellationToken)
        {
            RunSync(() => MoveAbsAsync(axisName, targetPos, MotionController.DefaultTimeoutMilliseconds, cancellationToken)).EnsureSuccess();
        }

        public void MoveAbs(IAxis axis, double targetPos)
        {
            SafeMove(MotionMoveRequest.ForAxis(axis, targetPos));
        }

        public void MoveAbs(IAxis axis, double targetPos, CancellationToken cancellationToken)
        {
            RunSync(() => MoveAbsAsync(axis, targetPos, MotionController.DefaultTimeoutMilliseconds, cancellationToken)).EnsureSuccess();
        }

        public void MoveRel(string axisName, double distance)
        {
            MoveRel(axisName, distance, CancellationToken.None);
        }

        public void MoveRel(string axisName, double distance, CancellationToken cancellationToken)
        {
            RunSync(() => MoveRelAsync(axisName, distance, MotionController.DefaultTimeoutMilliseconds, cancellationToken)).EnsureSuccess();
        }

        public void MoveRel(IAxis axis, double distance)
        {
            MoveRel(axis, distance, CancellationToken.None);
        }

        public void MoveRel(IAxis axis, double distance, CancellationToken cancellationToken)
        {
            RunSync(() => MoveRelAsync(axis, distance, MotionController.DefaultTimeoutMilliseconds, cancellationToken)).EnsureSuccess();
        }

        public void SafeHome(params string[] axisNames)
        {
            SafeHome(CancellationToken.None, axisNames);
        }

        public void SafeHome(CancellationToken cancellationToken, params string[] axisNames)
        {
            RunSync(() => SafeHomeAsync(cancellationToken, axisNames)).EnsureSuccess();
        }

        public void SafeHome(params IAxis[] axes)
        {
            SafeHome(CancellationToken.None, axes);
        }

        public void SafeHome(CancellationToken cancellationToken, params IAxis[] axes)
        {
            RunSync(() => SafeHomeAsync(cancellationToken, axes)).EnsureSuccess();
        }

        public void Home(string axisName)
        {
            Home(axisName, CancellationToken.None);
        }

        public void Home(string axisName, CancellationToken cancellationToken)
        {
            RunSync(() => HomeAsync(axisName, MotionController.DefaultTimeoutMilliseconds, cancellationToken)).EnsureSuccess();
        }

        public void Home(IAxis axis)
        {
            Home(axis, CancellationToken.None);
        }

        public void Home(IAxis axis, CancellationToken cancellationToken)
        {
            RunSync(() => HomeAsync(axis, MotionController.DefaultTimeoutMilliseconds, cancellationToken)).EnsureSuccess();
        }

        public void Stop(string axisName)
        {
            Stop(axisName, CancellationToken.None);
        }

        public void Stop(string axisName, CancellationToken cancellationToken)
        {
            RunSync(() => StopAsync(axisName, 3000, cancellationToken)).EnsureSuccess();
        }

        public void Stop(IAxis axis)
        {
            Stop(axis, CancellationToken.None);
        }

        public void Stop(IAxis axis, CancellationToken cancellationToken)
        {
            RunSync(() => StopAsync(axis, 3000, cancellationToken)).EnsureSuccess();
        }

        public void Stop(params string[] axisNames)
        {
            Stop(CancellationToken.None, axisNames);
        }

        public void Stop(CancellationToken cancellationToken, params string[] axisNames)
        {
            RunSync(() => StopAsync(3000, cancellationToken, axisNames)).EnsureSuccess();
        }

        public void Stop(params IAxis[] axes)
        {
            Stop(CancellationToken.None, axes);
        }

        public void Stop(CancellationToken cancellationToken, params IAxis[] axes)
        {
            RunSync(() => StopAsync(3000, cancellationToken, axes)).EnsureSuccess();
        }

        public async Task<MotionResult> MoveAbsAsync(string axisName, double targetPos, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
        {
            var request = ResolveRequest(EnsureRequestBatchMetadata(MotionMoveRequest.ForAxisName(axisName, targetPos, timeoutMilliseconds, alarmId), null));
            await _motionPrecheckService.ValidateAsync(CreateSafetyContext(MotionRequestKind.MoveAbs, request), cancellationToken, alarmId).ConfigureAwait(false);
            return await MoveAbsInternalAsync(request, cancellationToken).ConfigureAwait(false);
        }

        public async Task<MotionResult> MoveAbsAsync(IAxis axis, double targetPos, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
        {
            var request = ResolveRequest(EnsureRequestBatchMetadata(MotionMoveRequest.ForAxis(axis, targetPos, timeoutMilliseconds, alarmId), null));
            await _motionPrecheckService.ValidateAsync(CreateSafetyContext(MotionRequestKind.MoveAbs, request), cancellationToken, alarmId).ConfigureAwait(false);
            return await MoveAbsInternalAsync(request, cancellationToken).ConfigureAwait(false);
        }

        public async Task<MotionResult> MoveRelAsync(string axisName, double distance, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
        {
            var request = ResolveRequest(EnsureRequestBatchMetadata(MotionMoveRequest.ForRelativeAxisName(axisName, distance, timeoutMilliseconds, alarmId), null));
            await _motionPrecheckService.ValidateAsync(CreateSafetyContext(MotionRequestKind.MoveRel, request), cancellationToken, alarmId).ConfigureAwait(false);
            return await MoveRelInternalAsync(request, cancellationToken).ConfigureAwait(false);
        }

        public async Task<MotionResult> MoveRelAsync(IAxis axis, double distance, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
        {
            var request = ResolveRequest(EnsureRequestBatchMetadata(MotionMoveRequest.ForRelativeAxis(axis, distance, timeoutMilliseconds, alarmId), null));
            await _motionPrecheckService.ValidateAsync(CreateSafetyContext(MotionRequestKind.MoveRel, request), cancellationToken, alarmId).ConfigureAwait(false);
            return await MoveRelInternalAsync(request, cancellationToken).ConfigureAwait(false);
        }

        public Task<MotionBatchResult> SafeHomeAsync(params string[] axisNames)
        {
            return SafeHomeAsync(CancellationToken.None, axisNames);
        }

        public Task<MotionBatchResult> SafeHomeAsync(params IAxis[] axes)
        {
            return SafeHomeAsync(CancellationToken.None, axes);
        }

        public async Task<MotionBatchResult> SafeHomeAsync(CancellationToken cancellationToken, params string[] axisNames)
        {
            if (axisNames == null || axisNames.Length == 0)
            {
                return new MotionBatchResult(Array.Empty<MotionResult>());
            }

            return await SafeHomeAsync(cancellationToken, axisNames.Select(ResolveAxis).ToArray()).ConfigureAwait(false);
        }

        public async Task<MotionBatchResult> SafeHomeAsync(CancellationToken cancellationToken, params IAxis[] axes)
        {
            if (axes == null || axes.Length == 0)
            {
                return new MotionBatchResult(Array.Empty<MotionResult>());
            }

            var batchId = CreateBatchId(axes.Select(x => x != null ? x.Name : null));
            var resolvedAxes = axes.Select(EnsureAxis).ToArray();
            await _motionPrecheckService.ValidateAsync(CreateHomeSafetyContext(resolvedAxes, timeoutMilliseconds: null, batchId: batchId), cancellationToken, null).ConfigureAwait(false);
            using (var linkedCancellationTokenSource = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken))
            {
                var tasks = resolvedAxes.Select(x =>
                {
                    var homeRequest = BuildHomeRequest(x, MotionController.DefaultTimeoutMilliseconds, null, batchId);
                    return HomeInternalAsync(homeRequest, linkedCancellationTokenSource.Token, linkedCancellationTokenSource, resolvedAxes);
                }).ToArray();
                var results = await Task.WhenAll(tasks).ConfigureAwait(false);
                return new MotionBatchResult(results);
            }
        }

        public async Task<MotionResult> HomeAsync(string axisName, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
        {
            var axis = ResolveAxis(axisName);
            var request = BuildHomeRequest(axis, timeoutMilliseconds, alarmId, null);
            await _motionPrecheckService.ValidateAsync(CreateHomeSafetyContext(new[] { axis }, timeoutMilliseconds, request.BatchId), cancellationToken, alarmId).ConfigureAwait(false);
            return await HomeInternalAsync(request, cancellationToken, null, null).ConfigureAwait(false);
        }

        public async Task<MotionResult> HomeAsync(IAxis axis, int timeoutMilliseconds = MotionController.DefaultTimeoutMilliseconds, CancellationToken cancellationToken = default(CancellationToken), int? alarmId = null)
        {
            var resolvedAxis = EnsureAxis(axis);
            var request = BuildHomeRequest(resolvedAxis, timeoutMilliseconds, alarmId, null);
            await _motionPrecheckService.ValidateAsync(CreateHomeSafetyContext(new[] { resolvedAxis }, timeoutMilliseconds, request.BatchId), cancellationToken, alarmId).ConfigureAwait(false);
            return await HomeInternalAsync(request, cancellationToken, null, null).ConfigureAwait(false);
        }

        public Task<MotionResult> StopAsync(string axisName, int timeoutMilliseconds = 3000, CancellationToken cancellationToken = default(CancellationToken))
        {
            return StopAsync(ResolveAxis(axisName), timeoutMilliseconds, cancellationToken);
        }

        public async Task<MotionResult> StopAsync(IAxis axis, int timeoutMilliseconds = 3000, CancellationToken cancellationToken = default(CancellationToken))
        {
            var resolvedAxis = EnsureAxis(axis);
            var controller = _controllerRegistry.GetController(resolvedAxis);
            var normalizedTimeout = NormalizeStopTimeout(timeoutMilliseconds);
            var result = await controller.StopAsync(normalizedTimeout, cancellationToken).ConfigureAwait(false);
            return result.WithFailureDetails(result.FailureStage, result.TimedOut, result.StoppedByCoordinator, Guid.NewGuid().ToString("N"), null);
        }

        public Task<MotionBatchResult> StopAsync(params string[] axisNames)
        {
            return StopAsync(3000, CancellationToken.None, axisNames);
        }

        public Task<MotionBatchResult> StopAsync(params IAxis[] axes)
        {
            return StopAsync(3000, CancellationToken.None, axes);
        }

        public async Task<MotionBatchResult> StopAsync(int timeoutMilliseconds, CancellationToken cancellationToken, params string[] axisNames)
        {
            if (axisNames == null || axisNames.Length == 0)
            {
                return new MotionBatchResult(Array.Empty<MotionResult>());
            }

            return await StopAsync(timeoutMilliseconds, cancellationToken, axisNames.Select(ResolveAxis).ToArray()).ConfigureAwait(false);
        }

        public async Task<MotionBatchResult> StopAsync(int timeoutMilliseconds, CancellationToken cancellationToken, params IAxis[] axes)
        {
            if (axes == null || axes.Length == 0)
            {
                return new MotionBatchResult(Array.Empty<MotionResult>());
            }

            var resolvedAxes = axes.Select(EnsureAxis).ToArray();
            var tasks = resolvedAxes.Select(x => StopAsync(x, timeoutMilliseconds, cancellationToken)).ToArray();
            var results = await Task.WhenAll(tasks).ConfigureAwait(false);
            return new MotionBatchResult(results);
        }
        #endregion

        #region ??????
        private async Task<MotionResult> MoveAbsInternalAsync(ResolvedMotionMoveRequest resolvedRequest, CancellationToken cancellationToken, CancellationTokenSource coordinatedCancellationTokenSource = null, IReadOnlyCollection<IAxis> relatedAxes = null)
        {
            if (resolvedRequest == null)
            {
                throw new ArgumentNullException(nameof(resolvedRequest));
            }

            var request = resolvedRequest.Request;
            var axis = resolvedRequest.Axis;
            var controller = _controllerRegistry.GetController(axis);
            var timeoutMilliseconds = NormalizeTimeout(request.TimeoutMilliseconds);

            var result = await controller.MoveAbsAsync(request.TargetPosition, timeoutMilliseconds, cancellationToken, request.PositionTolerance).ConfigureAwait(false);
            result = ApplyRequestMetadata(result, request);
            if (result.Succeeded || result.Cancelled)
            {
                return result;
            }

            CancelAndStopRelatedAxes(coordinatedCancellationTokenSource, relatedAxes, axis.Name, result.Exception ?? new InvalidOperationException(result.Message), request.StopOnFailure);
            if (!request.AlarmId.HasValue)
            {
                return result.WithFailureDetails("Execute", result.TimedOut, coordinatedCancellationTokenSource != null && coordinatedCancellationTokenSource.IsCancellationRequested, request.CorrelationId, request.BatchId);
            }

            return result.WithAlarmReported(await _motionAlarmReporter.ReportAlarmAsync(request.AlarmId).ConfigureAwait(false))
                .WithFailureDetails("Execute", result.TimedOut, coordinatedCancellationTokenSource != null && coordinatedCancellationTokenSource.IsCancellationRequested, request.CorrelationId, request.BatchId);
        }

        private async Task<MotionResult> MoveRelInternalAsync(ResolvedMotionMoveRequest resolvedRequest, CancellationToken cancellationToken, CancellationTokenSource coordinatedCancellationTokenSource = null, IReadOnlyCollection<IAxis> relatedAxes = null)
        {
            if (resolvedRequest == null)
            {
                throw new ArgumentNullException(nameof(resolvedRequest));
            }

            var request = resolvedRequest.Request;
            var axis = resolvedRequest.Axis;
            var controller = _controllerRegistry.GetController(axis);
            var timeoutMilliseconds = NormalizeTimeout(request.TimeoutMilliseconds);

            var result = await controller.MoveRelAsync(request.TargetPosition, timeoutMilliseconds, cancellationToken, request.PositionTolerance).ConfigureAwait(false);
            result = ApplyRequestMetadata(result, request);
            if (result.Succeeded || result.Cancelled)
            {
                return result;
            }

            CancelAndStopRelatedAxes(coordinatedCancellationTokenSource, relatedAxes, axis.Name, result.Exception ?? new InvalidOperationException(result.Message), request.StopOnFailure);
            if (!request.AlarmId.HasValue)
            {
                return result.WithFailureDetails("Execute", result.TimedOut, coordinatedCancellationTokenSource != null && coordinatedCancellationTokenSource.IsCancellationRequested, request.CorrelationId, request.BatchId);
            }

            return result.WithAlarmReported(await _motionAlarmReporter.ReportAlarmAsync(request.AlarmId).ConfigureAwait(false))
                .WithFailureDetails("Execute", result.TimedOut, coordinatedCancellationTokenSource != null && coordinatedCancellationTokenSource.IsCancellationRequested, request.CorrelationId, request.BatchId);
        }

        private async Task<MotionResult> HomeInternalAsync(MotionMoveRequest request, CancellationToken cancellationToken, CancellationTokenSource coordinatedCancellationTokenSource = null, IReadOnlyCollection<IAxis> relatedAxes = null)
        {
            if (request == null)
            {
                throw new ArgumentNullException(nameof(request));
            }

            var axis = EnsureAxis(request.Axis ?? ResolveAxis(request.AxisName));
            var controller = _controllerRegistry.GetController(axis);
            var result = await controller.HomeAsync(NormalizeTimeout(request.TimeoutMilliseconds), cancellationToken).ConfigureAwait(false);
            result = ApplyRequestMetadata(result, request);
            if (result.Succeeded || result.Cancelled)
            {
                return result;
            }

            CancelAndStopRelatedAxes(coordinatedCancellationTokenSource, relatedAxes, axis.Name, result.Exception ?? new InvalidOperationException(result.Message), request.StopOnFailure);
            if (!request.AlarmId.HasValue)
            {
                return result.WithFailureDetails("Execute", result.TimedOut, coordinatedCancellationTokenSource != null && coordinatedCancellationTokenSource.IsCancellationRequested, request.CorrelationId, request.BatchId);
            }

            return result.WithAlarmReported(await _motionAlarmReporter.ReportAlarmAsync(request.AlarmId).ConfigureAwait(false))
                .WithFailureDetails("Execute", result.TimedOut, coordinatedCancellationTokenSource != null && coordinatedCancellationTokenSource.IsCancellationRequested, request.CorrelationId, request.BatchId);
        }

        private ResolvedMotionMoveRequest ResolveRequest(MotionMoveRequest request)
        {
            if (request == null)
            {
                throw new ArgumentNullException(nameof(request));
            }

            var axis = request.Axis ?? ResolveAxis(request.AxisName);
            return new ResolvedMotionMoveRequest(request, axis);
        }

        private static IAxis EnsureAxis(IAxis axis)
        {
            if (axis == null)
            {
                throw new ArgumentNullException(nameof(axis));
            }

            return axis;
        }

        private MotionSafetyContext CreateSafetyContext(MotionRequestKind requestKind, params ResolvedMotionMoveRequest[] resolvedRequests)
        {
            var requests = (resolvedRequests ?? Array.Empty<ResolvedMotionMoveRequest>()).Select(x => x.Request).ToArray();
            var axes = (resolvedRequests ?? Array.Empty<ResolvedMotionMoveRequest>()).Select(x => x.Axis).ToArray();
            var timeout = requests.Length == 0 ? (int?)null : requests.Max(x => x.TimeoutMilliseconds);
            var source = requests.Select(x => x.Source).FirstOrDefault(x => !string.IsNullOrWhiteSpace(x));
            return new MotionSafetyContext(requestKind, axes, requests, source, timeout);
        }

        private MotionSafetyContext CreateHomeSafetyContext(IReadOnlyCollection<IAxis> axes, int? timeoutMilliseconds = null, string batchId = null)
        {
            var homeRequests = (axes ?? Array.Empty<IAxis>())
                .Select(x => BuildHomeRequest(x, timeoutMilliseconds ?? MotionController.DefaultTimeoutMilliseconds, null, batchId))
                .ToArray();

            return new MotionSafetyContext(MotionRequestKind.Home, axes ?? Array.Empty<IAxis>(), homeRequests, "SafeHome", timeoutMilliseconds);
        }

        private MotionMoveRequest BuildHomeRequest(IAxis axis, int timeoutMilliseconds, int? alarmId, string batchId)
        {
            var resolvedAxis = EnsureAxis(axis);
            var homeRequest = MotionMoveRequest.ForAxis(
                resolvedAxis,
                resolvedAxis.State != null ? resolvedAxis.State.ActualPos : resolvedAxis.GetPositionImmediate(),
                timeoutMilliseconds,
                alarmId,
                "SafeHome",
                new[] { "Home" },
                null,
                batchId,
                null,
                true);

            return EnsureRequestBatchMetadata(homeRequest, batchId);
        }

        private static MotionMoveRequest EnsureRequestBatchMetadata(MotionMoveRequest request, string batchId)
        {
            if (request == null)
            {
                throw new ArgumentNullException(nameof(request));
            }

            var ensuredCorrelationId = string.IsNullOrWhiteSpace(request.CorrelationId) ? Guid.NewGuid().ToString("N") : request.CorrelationId;
            var ensuredBatchId = string.IsNullOrWhiteSpace(request.BatchId) ? batchId : request.BatchId;
            return request.WithMetadata(correlationId: ensuredCorrelationId, batchId: ensuredBatchId);
        }

        private static MotionResult ApplyRequestMetadata(MotionResult result, MotionMoveRequest request)
        {
            if (result == null)
            {
                throw new ArgumentNullException(nameof(result));
            }

            if (request == null)
            {
                return result;
            }

            return result.WithFailureDetails(result.FailureStage, result.TimedOut, result.StoppedByCoordinator, request.CorrelationId, request.BatchId);
        }

        private static string CreateBatchId(IEnumerable<string> seedValues)
        {
            var normalized = (seedValues ?? Enumerable.Empty<string>()).Where(x => !string.IsNullOrWhiteSpace(x)).ToArray();
            return normalized.Length == 0 ? Guid.NewGuid().ToString("N") : string.Format("{0}-{1}", DateTime.UtcNow.ToString("yyyyMMddHHmmssfff"), Math.Abs(string.Join("|", normalized).GetHashCode()));
        }

        private static string CreateBatchId(IEnumerable<MotionMoveRequest> requests)
        {
            return CreateBatchId((requests ?? Enumerable.Empty<MotionMoveRequest>()).Select(x => x != null ? x.AxisName : null));
        }

        private static int NormalizeTimeout(int timeoutMilliseconds)
        {
            return timeoutMilliseconds > 0 ? timeoutMilliseconds : MotionController.DefaultTimeoutMilliseconds;
        }

        private static int NormalizeStopTimeout(int timeoutMilliseconds)
        {
            return timeoutMilliseconds > 0 ? timeoutMilliseconds : 3000;
        }

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

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

        private void CancelAndStopRelatedAxes(CancellationTokenSource coordinatedCancellationTokenSource, IReadOnlyCollection<IAxis> relatedAxes, string failedAxisName, Exception exception, bool stopOnFailure)
        {
            if (!stopOnFailure)
            {
                return;
            }

            if (coordinatedCancellationTokenSource != null && !coordinatedCancellationTokenSource.IsCancellationRequested)
            {
                coordinatedCancellationTokenSource.Cancel();
            }

            if (relatedAxes == null || relatedAxes.Count <= 1)
            {
                return;
            }

            string.Format("Axis:{0} move failed, stopping related axes. Error:{1}", failedAxisName, exception.Message).LogSysError();

            foreach (var axis in relatedAxes)
            {
                if (axis == null)
                {
                    continue;
                }

                try
                {
                    _controllerRegistry.GetController(axis).StopAsync(3000, CancellationToken.None).GetAwaiter().GetResult();
                }
                catch (Exception stopException)
                {
                    string.Format("Axis:{0} stop failed during coordinated stop. Error:{1}", axis.Name, stopException.Message).LogSysError();
                }
            }
        }

        private IAxis ResolveAxis(string axisName)
        {
            if (string.IsNullOrWhiteSpace(axisName))
            {
                throw new ArgumentNullException(nameof(axisName));
            }

            var axis = _hardware.GetAxisByName(axisName);
            if (axis == null)
            {
                throw new ArgumentException(string.Format("Axis with name {0} not found.", axisName), nameof(axisName));
            }

            return axis;
        }
        #endregion
    }
}