using MathNet.Numerics;
using Prism.Events;
using Prism.Ioc;
using Prism.Mvvm;
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using TeamAAS_VP.Core;
using TeamAAS_VP.Core.PLCs;
using TeamAAS_VP.Events;
using TeamAAS_VP.Interfaces;
using TeamAAS_VP.Models;
using TeamAAS_VP.Models.PLC;
using TeamAAS_VP.Models.Torque;

namespace TeamAAS_VP.Services
{
    public class TorqueService : BindableBase
    {
        IEventAggregator _eventAggregator;
        IConfigService _configService;
        IContainerProvider _container;
        IRobotService _robotService;
        IPlcService _plcService;
        private Management management;



        public TorqueService(IEventAggregator ea, IConfigService configService,
            IContainerProvider container, IRobotService robotService, IPlcService plcService)
        {
            _eventAggregator = ea;
            _configService = configService;
            _container = container;
            _robotService = robotService;
            _plcService = plcService;
            management = _container.Resolve<Management>();
        }

        public async Task<bool> Throw_Screw_Async(IRobot Robot, OpcUaClientPLC plc, PlcAddressConfig addressConfig, TorqueProduceModel PlcPoint)
        {
            List<bool> result = new List<bool>();
            var point = new RPoint()
            {
                X = PlcPoint.TorquePoints[0].X_Position,
                Y = PlcPoint.TorquePoints[0].Y_Position,
                Z = PlcPoint.TorquePoints[0].Z_Position_Start,
            };

            result.Add(await plc.WriteNodeAsync(addressConfig.Manu_ZAxis_Velocity.Address, (float)99.0));

            float Z_Distance = -(PlcPoint.TorquePoints[0].Z_Position_Start - PlcPoint.TorquePoints[0].Z_Position_Stop);

            result.Add(await Robot.JumpAsync(point, 0));

            result.Add(await plc.WriteNodeAsync<bool>(addressConfig.ManuToHome.Address, true));

            return !result.Contains(false);
        }

        public async Task<bool> Pick_Screw_Async(IRobot Robot, OpcUaClientPLC plc, PlcAddressConfig addressConfig, TorqueProduceModel PlcPoint)
        {
            List<bool> result = new List<bool>();
            var point = new RPoint()
            {
                X = PlcPoint.TorquePoints[1].X_Position,
                Y = PlcPoint.TorquePoints[1].Y_Position,
                Z = PlcPoint.TorquePoints[1].Z_Position_Start,
            };

            result.Add(await plc.WriteNodeAsync(addressConfig.Screw_Backward.Address, true));

            result.Add(await Robot.JumpAsync(point, 0));

            result.Add(await plc.WriteNodeAsync(addressConfig.ManuTOWork.Address, true));

            result.Add(await plc.WriteNodeAsync(addressConfig.Screw_Backward.Address, false));

            result.Add(await BackZero_Async(Robot, point));

            result.Add(await Task.Run(() =>
            {
                return (bool)plc.ReadNode(addressConfig.InWork.Address);
            }));

            return !result.Contains(false);
        }

        public async Task<bool> Screw_Torque_Async(IRobot Robot, OpcUaClientPLC plc, PlcAddressConfig addressConfig, TorqueProduceModel PlcPoint)
        {
            List<bool> result = new List<bool>();
            var point = new RPoint()
            {
                X = PlcPoint.TorquePoints[2].X_Position,
                Y = PlcPoint.TorquePoints[2].Y_Position,
                Z = PlcPoint.TorquePoints[2].Z_Position_Start,
            };
            //float Z_Distance = -(PlcPoint.TorquePoints[2].Z_Position_Start - PlcPoint.TorquePoints[2].Z_Position_Stop);

            result.Add(await plc.WriteNodeAsync(addressConfig.Manu_ZAxis_Velocity.Address, (float)5.0));

            result.Add(await Robot.JumpAsync(point, 0));

            result.Add(await plc.WriteNodeAsync(addressConfig.Check_ScrewTorque.Address, true));

            //result.Add(await Robot.JogAsync("Z", Z_Distance));
            point.Z = PlcPoint.TorquePoints[2].Z_Position_Stop;

            Robot.Go(point);

            result.Add(await plc.WriteNodeAsync(addressConfig.ManuToHome.Address, true));

            result.Add(await plc.WriteNodeAsync(addressConfig.Manu_ZAxis_Velocity.Address, (float)100.0));

            result.Add(await BackZero_Async(Robot, point));

            return !result.Contains(false);
        }

        
        public async Task<bool> Safety_Position_Async(IRobot Robot, OpcUaClientPLC plc, PlcAddressConfig addressConfig, TorqueProduceModel PlcPoint)
        {
            List<bool> result = new List<bool>();
            var point = new RPoint()
            {
                X = PlcPoint.TorquePoints[3].X_Position,
                Y = PlcPoint.TorquePoints[3].Y_Position,
                Z = PlcPoint.TorquePoints[3].Z_Position_Start,
            };

            result.Add( await Robot.JumpAsync(point, 0));
            
            return !result.Contains(false);
        }

        public async Task<bool> BackZero_Async(IRobot Robot, RPoint point)
        {
            point.Z = 0;
            return Robot.Go(point);
        }

        public async Task Signal_Move_Async(IRobot Robot, OpcUaClientPLC plc, PlcAddressConfig addressConfig, PlcPoint_Torque target)
        {
            var point = new RPoint()
            {
                X = target.X_Position,
                Y = target.Y_Position,
                Z = target.Z_Position_Start,
            };

            await Robot.JumpAsync(point, 0);

        }

    }

}