using Autofac;
using Bro.Common.Base;
using Bro.Common.Helper;
using Bro.Common.Interface;
using Bro.Common.Model;
using Bro.Device.AuboRobot;
using Bro.Device.SeerAGV;
using HalconDotNet;
using Newtonsoft.Json;
using System;
using System.Collections.Generic;
using System.Collections.Specialized;
using System.ComponentModel;
using System.Linq;
using System.Threading;
using System.Threading.Tasks;

namespace A032.Process
{
    public class TrayTask : IComplexDisplay
    {
        [Browsable(false)]
        public string TaskId { get; set; } = Guid.NewGuid().ToString();

        /// <summary>
        /// 优先级越高，越快执行
        /// </summary>
        [Category("任务配置")]
        [Description("优先级。优先级越高，越快执行")]
        public int Priority { get; set; }

        private string locationCode = "";

        [Category("任务配置")]
        [Description("任务执行地址代码")]
        [TypeConverter(typeof(PositionCodeConverter))]
        public string LocationCode
        {
            get => locationCode;
            set
            {
                if (locationCode != value)
                {
                    locationCode = value;

                    using (var scope = GlobalVar.Container.BeginLifetimeScope())
                    {
                        ProcessConfig config = scope.Resolve<ProcessConfig>();

                        Location = config.PositionCollection.FirstOrDefault(u => u.PositionCode == locationCode);
                    }
                }
            }
        }

        [Browsable(false)]
        public PathPosition Location { get; set; } = new PathPosition();

        private TaskType taskType = TaskType.LoadEmptyTrayToAGV;
        [Category("任务配置")]
        [Description("任务类型")]
        public TaskType TaskType
        {
            get => taskType;
            set
            {
                taskType = value;

                var attr = taskType.GetEnumAttribute<PriorityAttribute>();
                if (attr != null)
                {
                    Priority = attr.Priority;
                }
            }
        }

        private string sourceDeviceId = "";
        [Category("任务配置")]
        [Description("任务来源设备")]
        [TypeConverter(typeof(AllDeviceIdConverter))]
        public string SourceDeviceId
        {
            get => sourceDeviceId;
            set
            {
                if (sourceDeviceId != value)
                {
                    sourceDeviceId = value;

                    using (var scope = GlobalVar.Container.BeginLifetimeScope())
                    {
                        List<IDevice> deviceList = scope.Resolve<List<IDevice>>();

                        var device = deviceList.FirstOrDefault(u => u.Id == value);
                        SourceDeviceName = device.Name;
                    }
                }
            }
        }

        [Browsable(false)]
        [JsonIgnore]
        public string SourceDeviceName { get; set; } = "";

        [Browsable(false)]
        [JsonIgnore]
        public string BindId { get; set; } = "";

        [Browsable(false)]
        [JsonIgnore]
        public int WaitShift { get; set; } = 0;

        public string GetDisplayText()
        {
            return $"{SourceDeviceName}发起的{TaskType.ToString()}任务";
        }
    }

    public enum TaskType
    {
        [Priority(30)]
        UnloadEmptyTrayToMachine,

        [Priority(20)]
        LoadEmptyTrayToAGV,

        [Priority(30)]
        LoadFullTrayFromMachine,

        [Priority(20)]
        UnloadFullTrayToLine,

        [Priority(50)]
        Charge,

        [Priority(10)]
        IdleCharge,
    }

    public class PriorityAttribute : Attribute
    {
        public int Priority { get; set; }

        public PriorityAttribute(int priority)
        {
            Priority = priority;
        }
    }

    public partial class ProcessControl
    {
        private bool isEnableExecuteTask = true;
        public bool IsEnableExecuteTask
        {
            get => isEnableExecuteTask;
            set
            {
                isEnableExecuteTask = value;

                LogAsync(DateTime.Now, $"Set IsEnableExecuteTask {value.ToString()}", $"{(value ? "打开" : "关闭")}任务执行");
            }
        }
        List<AGVState> _disableStates = new List<AGVState>() { AGVState.InCharge, AGVState.Warning, AGVState.Unknown };

        private void Reset(string bindId = "")
        {
            WarningRemains.Clear();
            IsEnableExecuteTask = true;

            if (string.IsNullOrWhiteSpace(bindId))
            {
                Config.AGVBindCollection.ForEach(bind =>
                {
                    if (bind.UnitState == AGVState.Warning)
                    {
                        //bind.WarningMsg.Clear();
                        //bind.UnitState = AGVState.Idle;
                        bind.Reset();
                    }
                });

                LogAsync(DateTime.Now, "Reset", "执行全体复位操作");
            }
            else
            {
                var bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == bindId);

                if (bind != null && bind.UnitState == AGVState.Warning)
                {
                    //bind.WarningMsg.Clear();
                    //bind.UnitState = AGVState.Idle;

                    bind.Reset();

                    LogAsync(DateTime.Now, "Reset", $"执行{bind.AGV.Name}复位操作");
                }
            }
        }

        static object _taskLock = new object();
        NoticedList<TrayTask> TrayTaskCollection = new NoticedList<TrayTask>();

        private void InsertTask(TrayTask task)
        {
            lock (_taskLock)
            {
                if (TrayTaskCollection.Any(u => u.TaskType == task.TaskType && u.SourceDeviceId == task.SourceDeviceId))
                {
                    LogAsync(DateTime.Now, "重复任务", $"{task.SourceDeviceName}发起的{task.TaskType.ToString()}任务已存在任务列表中");
                    return;
                }

                int insertIndex = TrayTaskCollection.Count;
                var nextTask = TrayTaskCollection.FirstOrDefault(u => u.Priority < task.Priority && u.WaitShift == 0);
                if (nextTask != null)
                {
                    insertIndex = TrayTaskCollection.IndexOf(nextTask);
                }

                TrayTaskCollection.Insert(insertIndex, task);
            }
        }

        #region 任务触发
        private async void OnTaskListChanged(NotifyCollectionChangedAction action, List<TrayTask> task)
        {
            await Task.Run(() =>
            {
                if (action == NotifyCollectionChangedAction.Add)
                {
                    ExecuteTask();
                }
            });
        }

        private async void OnUnitStateChanged(string unitId, AGVState preState, AGVState currentState)
        {
            await Task.Run(() =>
            {
                if (currentState == AGVState.Idle || currentState == AGVState.IdleCharge)
                {
                    ExecuteTask(unitId);
                }
            });
        }

        private void ExecuteTask(string unitId = "")
        {
            TrayTask task = null;

            lock (_taskLock)
            {
                if (TrayTaskCollection.Count <= 0)
                {
                    return;
                }

                task = TrayTaskCollection.FirstOrDefault(u => string.IsNullOrWhiteSpace(u.BindId));
            }

            //任务是否已在执行中
            if (Config.AGVBindCollection.Any(u => u.CurrentTaskId == task.TaskId))
            {
                LogAsync(DateTime.Now, "任务执行中", $"{task.GetDisplayText()}正在执行中");
                return;
            }

            AGVBindUnit bind = null;

            if (string.IsNullOrWhiteSpace(unitId))
            {
                var available = Config.AGVBindCollection.Where(u => u.UnitState == AGVState.Idle || u.UnitState == AGVState.IdleCharge);

                switch (task.TaskType)
                {
                    case TaskType.LoadFullTrayFromMachine:
                        available = available.Where(u => u.FullTrayNum < Config.AGVAvailableTrayNums);
                        break;
                    case TaskType.UnloadEmptyTrayToMachine:
                        available = available.Where(u => u.EmptyTrayNum > 0);
                        break;
                    default:
                        break;
                }

                bind = available.FirstOrDefault();
            }
            else
            {
                bind = Config.AGVBindCollection.FirstOrDefault(u => u.Id == unitId && (u.UnitState == AGVState.Idle || u.UnitState == AGVState.IdleCharge));
            }

            if (bind == null)
            {
                LogAsync(DateTime.Now, $"暂时没有可执行任务的单元", "");
                ReArrangeTask(task);
                return;
            }

            if (task.Location == null)
            {
                switch (task.TaskType)
                {
                    case TaskType.LoadEmptyTrayToAGV:
                        task.Location = Config.PositionCollection.FirstOrDefault(u => !u.IsOccupied && u.Description == PathPositionDefinition.LoadEmptyTray);
                        break;
                    case TaskType.LoadFullTrayFromMachine:
                        task.Location = Config.PositionCollection.FirstOrDefault(u => !u.IsOccupied && u.Description == PathPositionDefinition.LoadFullTray && u.DeviceOwner == task.SourceDeviceId);
                        break;
                    case TaskType.UnloadEmptyTrayToMachine:
                        task.Location = Config.PositionCollection.FirstOrDefault(u => !u.IsOccupied && u.Description == PathPositionDefinition.UnloadEmptyTray && u.DeviceOwner == task.SourceDeviceId);
                        break;
                    case TaskType.UnloadFullTrayToLine:
                        task.Location = Config.PositionCollection.FirstOrDefault(u => !u.IsOccupied && u.Description == PathPositionDefinition.UnloadFullTray && u.DeviceOwner == task.SourceDeviceId);
                        break;
                    default:
                        break;
                }
            }

            if (task.Location == null)
            {
                ReArrangeTask(task);
                return;
            }
            else
            {
                task.Location.IsOccupied = true;
            }

            task.BindId = bind.Id;

            if (!string.IsNullOrWhiteSpace(bind.CurrentTaskId))
            {
                bind.AGV.CancelTask();
            }

            LogAsync(DateTime.Now, $"开始执行任务", task.GetDisplayText());

            bind.IsTaskCancelled = false;
            bind.IsTaskCancelling = false;

            bind.UnitState = AGVState.Running;
            bind.CurrentTaskId = task.TaskId;

            try
            {
                switch (task.TaskType)
                {
                    case TaskType.LoadEmptyTrayToAGV:
                        LoadEmptyTrayToAGV(task, bind);
                        break;
                    case TaskType.LoadFullTrayFromMachine:
                        LoadFullTrayFromMachine(task, bind);
                        break;
                    case TaskType.UnloadEmptyTrayToMachine:
                        UnloadEmptyTrayToMachine(task, bind);
                        break;
                    case TaskType.UnloadFullTrayToLine:
                        UnloadFullTrayToLine(task, bind);
                        break;
                    default:
                        break;
                }

                ////任务完成后检查电量
                //bind.AGV.BatteryHandle.Reset();
                //bool isNotTimeout = bind.AGV.BatteryHandle.WaitOne((bind.AGV.InitialConfig as SeerAGVInitialConfig).ScanInterval * 10);

                //if (!isNotTimeout)
                //{
                //    throw new ProcessException($"{bind.AGV.Name}获取电池状态超时");
                //}
            }
            catch (TaskCanceledException ex)
            {
                try
                {
                    CancelTask(bind);
                }
                catch (Exception)
                {
                    ExceptionHandle(task, bind, ex, $"任务{task.TaskId}取消失败，");
                    return;
                }
                finally
                {
                    bind.IsTaskCancelled = false;
                    bind.IsTaskCancelling = false;
                }
            }
            catch (Exception ex)
            {
                ExceptionHandle(task, bind, ex);
                return;
            }
            finally
            {
                //lock (_taskListLock)
                //{
                //    _taskList.RemoveAt(0);
                //}
            }

            LogAsync(DateTime.Now, $"任务{task.TaskId}流程结束", "");

            AfterTaskHandle(task, bind);
        }

        private void ReArrangeTask(TrayTask task)
        {
            task.WaitShift++;

            TrayTaskCollection.Remove(task);

            if (task.WaitShift > Config.DefaultWaitShift)
            {
                LogAsync(DateTime.Now, "任务无法执行", $"{task.GetDisplayText()}等待{task.WaitShift}次无法执行，已移出任务队列");
            }
            else
            {
                InsertTask(task);
            }
        }

        //AutoResetEvent _taskDoneHandle = new AutoResetEvent(true);
        Dictionary<string, AutoResetEvent> _bindTaskDoneHandleDict = new Dictionary<string, AutoResetEvent>();
        private void AfterTaskHandle(TrayTask task, AGVBindUnit bind, string warningMsg = "", bool isWarningRaised = false)
        {
            lock (_taskLock)
            {
                task.Location.IsOccupied = false;
                TrayTaskCollection.Remove(task);
            }

            bind.CurrentTaskId = "";
            if (isWarningRaised)
            {
                bind.WarningMsg.Add(warningMsg);
                bind.UnitState = AGVState.Warning;
            }
            else
            {
                //任务完成后检查电量
                bind.AGV.BatteryHandle.Reset();
                bool isNotTimeout = bind.AGV.BatteryHandle.WaitOne((bind.AGV.InitialConfig as SeerAGVInitialConfig).ScanInterval * 10);

                if (!isNotTimeout)
                {
                    string msg = $"{bind.AGV.Name}获取电池状态超时";
                    bind.WarningMsg.Add(msg);
                    new ProcessException(msg);
                    bind.UnitState = AGVState.Warning;
                }
                else
                {
                    SeerAGVInitialConfig iConfig = bind.AGV.InitialConfig as SeerAGVInitialConfig;
                    if (bind.AGV.BatteryLvl <= iConfig.BatteryLvlToCharge)
                    {
                        bind.UnitState = AGVState.InCharge;

                        var chargePosition = Config.PositionCollection.FirstOrDefault(u => !u.IsOccupied && u.Description == PathPositionDefinition.Charge);

                        if (chargePosition == null)
                        {
                            string msg = $"{bind.AGV.Name}目前无可用充电地址";
                            bind.WarningMsg.Add(msg);
                            new ProcessException(msg);
                            bind.UnitState = AGVState.Warning;
                        }
                        else
                        {
                            bind.AGV.TaskOrder(chargePosition.PositionCode, true);
                        }
                    }
                    else
                    {
                        bind.UnitState = AGVState.Idle;
                    }
                }
            }

            _bindTaskDoneHandleDict[bind.Id].Set();
        }

        /// <summary>
        /// 取消当前任务，如果已取卷宗，归还到原处
        /// </summary>
        /// <param name="bind"></param>
        private void CancelTask(AGVBindUnit bind)
        {
            bind.IsTaskCancelling = true;

            bind.AGV.CancelTask();
            bind.Robot.Move(new RobotPoint(), MoveType.Origin, true);
        }

        /// <summary>
        /// 过程异常处理，反馈异常消息到RabbitMQ
        /// </summary>
        /// <param name="task"></param>
        /// <param name="bind"></param>
        /// <param name="ex"></param>
        /// <param name="errorMsg"></param>
        private void ExceptionHandle(TrayTask task, AGVBindUnit bind, Exception ex, string errorMsg = "")
        {
            bool isWarningEx = false;
            if (ex is ProcessException)
            {
                isWarningEx = (ex as ProcessException).Level > 0;
                errorMsg += (ex as ProcessException).ErrorCode;
            }
            else
            {
                isWarningEx = true;
                errorMsg += ex.Message;
                LogAsync(DateTime.Now, $"{task.TaskId}异常", ex.GetExceptionMessage());
            }

            try
            {
                SwitchLight(bind.Robot, false);
                bind.Robot.Move(new RobotPoint(), MoveType.Origin, true);
            }
            catch (Exception)
            {
            }

            LogAsync(DateTime.Now, $"任务{task.GetDisplayText()}异常反馈", errorMsg);

            AfterTaskHandle(task, bind, errorMsg, isWarningEx);
        }
        #endregion

        #region 任务执行
        /// <summary>
        /// 将满Tray放置到产线上
        /// </summary>
        /// <param name="task"></param>
        /// <param name="bind"></param>
        private void UnloadFullTrayToLine(TrayTask task, AGVBindUnit bind)
        {
            string methodName = "UnloadFullTrayToLine";

            bind.AGV.TaskOrder(task.Location.PositionCode, true);

            var visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.CameraId == bind.CameraId && u.PositionCode == task.Location.PositionCode);

            if (visionConfig == null)
                throw new ProcessException($"未能获取{bind.Camera.Name}在{task.Location.PositionCode}的视觉配置");

            while (bind.FullTrayNum > 0)
            {
                bind.Robot.Move(visionConfig.RobotSnapshotPoint, MoveType.AbsoluteMove, true);

                bool isLineReady = false;
                if (Config.IsEnableVisionGuide)
                {
                    int reTryTime = Config.LineBusyRetryTimes;

                    HDevEngineTool tool = null;
                    if (!_halconToolDict.ContainsKey(visionConfig.CameraOpConfig.AlgorithemPath))
                    {
                        throw new ProcessException($"未配置{methodName}的算法工具");
                    }

                    tool = _halconToolDict[visionConfig.CameraOpConfig.AlgorithemPath];

                    SwitchLight(bind.Robot, true);
                    Thread.Sleep(500);//等待灯开

                    do
                    {
                        RobotPoint point = new RobotPoint();
                        using (var hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, methodName))
                        {
                            tool.InputImageDic.Clear();
                            tool.InputImageDic["INPUT_Image"] = hImage;

                            tool.RunProcedure();

                            if (!tool.IsSuccessful)
                            {
                                throw new ProcessException($"{visionConfig.CameraOpConfig.AlgorithemPath}算法运行失败");
                            }

                            isLineReady = tool.GetResultTuple("OUTPUT_Result").I == 1;
                        }

                        if (!isLineReady)
                        {
                            LogAsync(DateTime.Now, $"{task.Location.PositionCode}位置线体繁忙", "");
                            Thread.Sleep(Config.LineBusyWaitInterval * 1000);
                            reTryTime--;
                        }
                        else
                        {
                            reTryTime = 0;
                        }
                    } while (reTryTime > 0);

                    SwitchLight(bind.Robot, false);
                }
                else
                {
                    isLineReady = true;
                }

                if (isLineReady)
                {
                    bind.Robot.UnLoad(bind.Robot.CurrentPoint, TrayType.FullTray, true);
                }
                else
                {
                    bind.Robot.Move(new RobotPoint(), MoveType.Origin, true);
                    break;
                }
            }
        }

        /// <summary>
        /// 将空Tray放置到压机上
        /// </summary>
        /// <param name="task"></param>
        /// <param name="bind"></param>
        private void UnloadEmptyTrayToMachine(TrayTask task, AGVBindUnit bind)
        {
            string methodName = "UnloadEmptyTrayToMachine";

            MachineRelatedOperation(task, bind, methodName);

            RobotPoint point = bind.Robot.CurrentPoint.DeepSerializeClone();

            int emptyTrayNum = 0;
            do
            {
                bind.Robot.UnLoad(point, TrayType.EmptyTray, true);
                emptyTrayNum++;

                bind.Robot.MonitorHandle.Reset();
                var isNotTimeout = bind.Robot.MonitorHandle.WaitOne((bind.Robot.InitialConfig as AuboRobotInitialConfig).ReplyTimeout * 3);
                if (!isNotTimeout)
                    throw new ProcessException($"{bind.Robot.Name}监听操作超时");

            } while (emptyTrayNum < Config.Machine_EmptyTrayNum && bind.EmptyTrayNum > 0);
        }

        /// <summary>
        /// 从压机上取满Tray到AGV
        /// </summary>
        /// <param name="task"></param>
        /// <param name="bind"></param>
        private void LoadFullTrayFromMachine(TrayTask task, AGVBindUnit bind)
        {
            string methodName = "LoadFullTrayFromMachine";

            MachineRelatedOperation(task, bind, methodName);

            RobotPoint point = bind.Robot.CurrentPoint.DeepSerializeClone();

            int fullTrayNum = Config.Machine_FullTrayNum;
            do
            {
                bind.Robot.Load(point, TrayType.FullTray, true);
                fullTrayNum--;
                bind.FullTrayNum++;

                bind.Robot.MonitorHandle.Reset();
                var isNotTimeout = bind.Robot.MonitorHandle.WaitOne((bind.Robot.InitialConfig as AuboRobotInitialConfig).ReplyTimeout * 3);
                if (!isNotTimeout)
                    throw new ProcessException($"{bind.Robot.Name}监听操作超时");

            } while (fullTrayNum > 0 && bind.FullTrayNum < Config.AGVAvailableTrayNums);
        }

        private void MachineRelatedOperation(TrayTask task, AGVBindUnit bind, string methodName)
        {
            bind.AGV.TaskOrder(task.Location.PositionCode, true);

            var visionConfig = Config.VisionConfigCollection.FirstOrDefault(u => u.CameraId == bind.CameraId && u.PositionCode == task.Location.PositionCode);

            if (visionConfig == null)
                throw new ProcessException($"未能获取{bind.Camera.Name}在{task.Location.PositionCode}的视觉配置");

            bind.Robot.Move(visionConfig.RobotSnapshotPoint, MoveType.AbsoluteMove, true);

            if (Config.IsEnableVisionGuide)
            {
                int repeatTime = Config.VisionGuideTimes;

                HDevEngineTool tool = null;
                if (!_halconToolDict.ContainsKey(visionConfig.CameraOpConfig.AlgorithemPath))
                {
                    throw new ProcessException($"未配置{methodName}的算法工具");
                }

                tool = _halconToolDict[visionConfig.CameraOpConfig.AlgorithemPath];

                SwitchLight(bind.Robot, true);
                Thread.Sleep(500);//等待灯开

                do
                {
                    RobotPoint point = new RobotPoint();
                    using (var hImage = CollectHImage(bind.Camera, visionConfig.CameraOpConfig, methodName))
                    {
                        tool.InputImageDic.Clear();
                        tool.InputImageDic["INPUT_Image"] = hImage;

                        tool.RunProcedure();

                        if (!tool.IsSuccessful)
                        {
                            throw new ProcessException($"{visionConfig.CameraOpConfig.AlgorithemPath}算法运行失败");
                        }

                        double du = tool.GetResultTuple("OUTPUT_X").D;
                        double dv = tool.GetResultTuple("OUTPUT_Y").D;

                        if (du < 0 || dv < 0)
                        {
                            throw new ProcessException($"{visionConfig.CameraOpConfig.AlgorithemPath}算法结果异常，点位信息获取失败");
                        }

                        du -= visionConfig.StandardPoint.X;
                        dv -= visionConfig.StandardPoint.Y;

                        HOperatorSet.AffineTransPoint2d(new HTuple(visionConfig.Matrix), du, dv, out HTuple dx, out HTuple dy);

                        point.X = (float)dx.D;
                        point.Y = (float)dy.D;
                        point.A = (float)tool.GetResultTuple("OUTPUT_Angle").D;
                    }

                    LogAsync(DateTime.Now, $"{methodName}引导坐标", point.GetDisplayText());

                    bind.Robot.Move(point, MoveType.RobotRelativeMove, true);

                    repeatTime--;
                } while (repeatTime > 0);

                SwitchLight(bind.Robot, false);

                //视觉导引后再做固定偏移
                bind.Robot.Move(visionConfig.RobotShift, MoveType.RobotRelativeMove, true);
            }
        }

        /// <summary>
        /// 人工装空Tray到AGV
        /// </summary>
        /// <param name="task"></param>
        /// <param name="bind"></param>
        private void LoadEmptyTrayToAGV(TrayTask task, AGVBindUnit bind)
        {
            bind.AGV.TaskOrder(task.Location.PositionCode, true);
            bind.Robot.Load(new RobotPoint(), TrayType.EmptyTray, true);
        }
        #endregion
    }
}