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

namespace MainShell.AlgorithmCalib.Service
{
    [Singleton]
    public class FusionCalibMotionService
    {
        private readonly HardwareManager _hardware;
        private readonly SafeAxisMotion _safeAxisMotion;

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

        public void SafeMove(IAxis axis, double position)
        {
            _safeAxisMotion.SafeMove(
                MotionMoveRequest.ForAxis(GetRequiredAxis(axis, nameof(axis)), position));
        }

        public void MoveWsAvoidance(double xPosition, double yPosition)
        {
            _safeAxisMotion.SafeMove(
                MotionMoveRequest.ForAxis(GetRequiredAxis(_hardware.Axis_X2, nameof(_hardware.Axis_X2)), xPosition),
                MotionMoveRequest.ForAxis(GetRequiredAxis(_hardware.Axis_Y1, nameof(_hardware.Axis_Y1)), yPosition));
        }

        public void MovePhsAvoidance(double xPosition, double yPosition)
        {
            _safeAxisMotion.SafeMove(
                MotionMoveRequest.ForAxis(GetRequiredAxis(_hardware.Axis_X1, nameof(_hardware.Axis_X1)), xPosition),
                MotionMoveRequest.ForAxis(GetRequiredAxis(_hardware.Axis_Y2, nameof(_hardware.Axis_Y2)), yPosition));
        }

        public void RotatePos(double rotatePosition)
        {
            _safeAxisMotion.SafeMove(
                MotionMoveRequest.ForAxis(GetRequiredAxis(_hardware.Axis_WS_R, nameof(_hardware.Axis_WS_R)), rotatePosition));
        }

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