using System;
using System.Collections.Generic;
using System.Linq;
using Aitex.Core.Common;
using Aitex.Core.RT.Log;
using Aitex.Core.RT.Routine;
using EFEM.RT.Routines;

using Aitex.Sorter.Common;
using Aitex.Sorter.RT.Module;
using MECF.Framework.Common.Equipment;
using MECF.Framework.Common.SubstrateTrackings;
using Aitex.Core.RT.Device;

namespace EFEM.RT.Modules
{
    public class DualArmMoveManager : IMoveManager
    {
        public event Action<MoveErrorArgument> OnMoveError;

        public int TaskCount { get { return tasks.Count; } }

        public bool IsPaused { get { return !_pause; } }
        public int WaferCompleted
        {
            get
            {
                int count = tasks.Count;
                int completed = lastTaskCount - count;
                lastTaskCount = count;
                return completed;
            }
        }


        private Queue<MoveTask> tasks = new Queue<MoveTask>();

        private Queue<MoveTask> curtasks = new Queue<MoveTask>();

        private Queue<IRoutine> steps = new Queue<IRoutine>();
        private IRoutine curStep = null;

        private readonly bool _isInterferencePrevent =
            DeviceDefineManager.Instance.GetValue<bool>("InterferencePrevent") ?? false;
        private IEfemPickRoutine pickRoutine1 = null;
        private IEfemPlaceRoutine placeRoutine1 = null;
        private IEfemPickRoutine pickRoutine2 = null;
        private IEfemPlaceRoutine placeRoutine2 = null;
        private SwapRoutine swaproutine1 = null;


        private int lastTaskCount = 0;
        private int handChangeCount = 0;


        private bool _pause;

        public DualArmMoveManager()
        {
            pickRoutine1 = new PickRoutine("System", "Pick");
            pickRoutine1.Initalize();
            placeRoutine1 =  new PlaceRoutine("System", "Place");
            placeRoutine1.Initalize();

            pickRoutine2 =  new PickRoutine("System", "Pick");
            pickRoutine2.Initalize();
            placeRoutine2 = new PlaceRoutine("System", "Place");
            placeRoutine2.Initalize();

            swaproutine1 = new SwapRoutine("System", "Swap");
            swaproutine1.Initalize();
        }

        public bool Initialize()
        {
            return true;
        }

        public bool Pause()
        {
            _pause = true;
            return true;
        }
        public bool Resume()
        {
            _pause = false;
            return true;
        }

        public bool Stop()
        {
            tasks.Clear();

            _pause = false;
            return true;
        }

        public bool Start(List<MoveTask> tasks)
        {
            _pause = false;

            //tasks = tasks.OrderBy(item =>item.SourceStaion).ThenBy(item =>  item.SourceSlot).ToList<MoveTask>();
            foreach (MoveTask task in tasks)
            {
                WaferManager.Instance.UpdateWaferProcessStatus(task.SourceStaion, task.SourceSlot, EnumWaferProcessStatus.Wait);//(task.WaferID, EnumWaferProcessStatus.Wait);
                task.WaferID = WaferManager.Instance.GetWaferID(task.SourceStaion, task.SourceSlot);
            }

            this.tasks.Clear();
            for (int i = 0; i < tasks.Count; i++)
            {
                this.tasks.Enqueue(tasks[i]);
            }

            if (!newTask())
            {
                return false;
            }


            if (steps.Count > 0)
            {
                curStep = steps.Dequeue();
            }

            if (curStep != null)
            {
                Result ret = curStep.Start();
                if (ret == Result.FAIL)
                {
                    return false;
                }

            }
            lastTaskCount = tasks.Count;
            return true;
        }

        private bool newTask()
        {
            MoveTask[] nextTasks = new MoveTask[] { null, null };
            steps.Clear();
            curStep = null;

            if (tasks.Count == 0)
            {
                return true;      //completed
            }

            for (int i = 0; i < nextTasks.Length && i < tasks.Count; i++)
            {
                nextTasks[i] = tasks.ElementAt(i);
            }          

            if(nextTasks[1] != null && IsNeighbors(nextTasks[0], nextTasks[1], true) && IsNeighbors(nextTasks[0], nextTasks[1], false))//dual arm
            {
                ////double pick double place
                DPDP(nextTasks[0], nextTasks[1]);
                WaferManager.Instance.UpdateWaferProcessStatus(nextTasks[0].SourceStaion, nextTasks[0].SourceSlot, EnumWaferProcessStatus.Wait);//(nextTasks[0].WaferID, EnumWaferProcessStatus.InProcess);
                WaferManager.Instance.UpdateWaferProcessStatus(nextTasks[1].SourceStaion, nextTasks[1].SourceSlot, EnumWaferProcessStatus.Wait);//(nextTasks[1].WaferID, EnumWaferProcessStatus.InProcess);
                tasks.Dequeue();
                tasks.Dequeue();

                LOG.Info(string.Format("Dual transfer step 1, wafer move from {0}{1:D2} to {2}{3:D2}.",
                          nextTasks[0].SourceStaion.ToString(), nextTasks[0].SourceSlot + 1, nextTasks[0].DestStaion.ToString(), nextTasks[0].DestSlot + 1));
                LOG.Info(string.Format("Dual transfer step 2, wafer move from {0}{1:D2} to {2}{3:D2}.",
                          nextTasks[1].SourceStaion.ToString(), nextTasks[1].SourceSlot + 1, nextTasks[1].DestStaion.ToString(), nextTasks[1].DestSlot + 1));
            }
            else
            {
                Hand hand;
                if (handChangeCount % 2 == 0)
                {
                    hand = WaferManager.Instance.CheckNoWafer(ModuleName.Robot, 0) ? Hand.Blade1 : Hand.Blade2;
                }
                else
                {
                    hand = WaferManager.Instance.CheckNoWafer(ModuleName.Robot, 1) ? Hand.Blade2 : Hand.Blade1;
                }
                handChangeCount++;
                if (handChangeCount == int.MaxValue)
                    handChangeCount = 0;

                if (!WaferManager.Instance.CheckNoWafer(ModuleName.Robot, (int)hand))
                {
                    return false;
                }

                Move(nextTasks[0], hand);
                WaferManager.Instance.UpdateWaferProcessStatus(nextTasks[0].SourceStaion, nextTasks[0].SourceSlot, EnumWaferProcessStatus.Wait);//(nextTasks[0].WaferID, EnumWaferProcessStatus.InProcess);
                tasks.Dequeue();

                LOG.Info(string.Format("Single transfer wafer move from {0}{1:D2} to {2}{3:D2}.",
                          nextTasks[0].SourceStaion.ToString(), nextTasks[0].SourceSlot, nextTasks[0].DestStaion.ToString(), nextTasks[0].DestSlot));
            }

            return true;
        }


        private bool IsSwap(MoveTask task)
        {
            if (task == null)
                return false;

            if ((task.DestStaion == task.SourceStaion) && (task.DestSlot == task.SourceSlot))
                return false;
            return WaferManager.Instance.CheckHasWafer(task.DestStaion, task.DestSlot);
        }


        private bool IsNeighbors(MoveTask task1, MoveTask task2, bool pick)
        {
            if (pick)
            {
                return (task1.SourceStaion == task2.SourceStaion) && (task1.SourceSlot + 1 == task2.SourceSlot);
            }
            return (task1.DestStaion == task2.DestStaion) && (task1.DestSlot + 1 == task2.DestSlot);
        }


        private void Move(MoveTask task, Hand hand)
        {
            if (task.SourceStaion != ModuleName.Robot)
            {
                pickRoutine1.Source = task.SourceStaion;
                pickRoutine1.Slot = task.SourceSlot;
                pickRoutine1.Blade = hand;
                steps.Enqueue(pickRoutine1);
            }
            if (task.DestStaion != ModuleName.Robot)
            {
                placeRoutine1.Station = task.DestStaion;
                placeRoutine1.Slot = task.DestSlot;
                placeRoutine1.Blade = hand;
                steps.Enqueue(placeRoutine1);
            }
        }

        private void DPDP(MoveTask task1, MoveTask task2)
        {
            if (task1.SourceStaion != ModuleName.Robot)
            {
                pickRoutine1.Source = task1.SourceStaion;
                pickRoutine1.Slot = task1.SourceSlot;
                pickRoutine1.Blade = Hand.Blade1;
                steps.Enqueue(pickRoutine1);
            }

            if (task2.SourceStaion != ModuleName.Robot)
            {
                pickRoutine2.Source = task2.SourceStaion;
                pickRoutine2.Slot = task2.SourceSlot;
                pickRoutine2.Blade = Hand.Blade2;
                steps.Enqueue(pickRoutine2);
            }

            if (task1.DestStaion != ModuleName.Robot)
            {
                placeRoutine1.Station = task1.DestStaion;
                placeRoutine1.Slot = task1.DestSlot;
                placeRoutine1.Blade = Hand.Blade1;
                steps.Enqueue(placeRoutine1);
            }

            if (task2.DestStaion != ModuleName.Robot)
            {
                placeRoutine2.Station = task2.DestStaion;
                placeRoutine2.Slot = task2.DestSlot;
                placeRoutine2.Blade = Hand.Blade2;
                steps.Enqueue(placeRoutine2);
            }
        }

        private void DPSP(MoveTask task1, MoveTask task2)
        {
            if (task1.SourceStaion != ModuleName.Robot)
            {
                pickRoutine1.Source = task1.SourceStaion;
                pickRoutine1.Slot = task1.SourceSlot;
                pickRoutine1.Blade = Hand.Blade1;
                steps.Enqueue(pickRoutine1);
            }

            if (task2.SourceStaion != ModuleName.Robot)
            {
                pickRoutine2.Source = task2.SourceStaion;
                pickRoutine2.Slot = task2.SourceSlot;
                pickRoutine2.Blade = Hand.Blade2;
                steps.Enqueue(pickRoutine2);
            }

            if (task1.DestStaion != ModuleName.Robot)
            {
                placeRoutine1.Station = task1.DestStaion;
                placeRoutine1.Slot = task1.DestSlot;
                placeRoutine1.Blade = Hand.Both;
                steps.Enqueue(placeRoutine1);
            }
        }


        private void SPDP(MoveTask task1, MoveTask task2)
        {
            if (task1.SourceStaion != ModuleName.Robot)
            {
                pickRoutine1.Source = task1.SourceStaion;
                pickRoutine1.Slot = task1.SourceSlot;
                pickRoutine1.Blade = Hand.Both;
                steps.Enqueue(pickRoutine1);
            }


            if (task1.DestStaion != ModuleName.Robot)
            {
                placeRoutine1.Station = task1.DestStaion;
                placeRoutine1.Slot = task1.DestSlot;
                placeRoutine1.Blade = Hand.Blade1;
                steps.Enqueue(placeRoutine1);
            }

            if (task2.DestStaion != ModuleName.Robot)
            {
                placeRoutine2.Station = task2.DestStaion;
                placeRoutine2.Slot = task2.DestSlot;
                placeRoutine2.Blade = Hand.Blade2;
                steps.Enqueue(placeRoutine2);
            }
        }

        private void SPSP(MoveTask task)
        {
            if (task.SourceStaion != ModuleName.Robot)
            {
                pickRoutine1.Source = task.SourceStaion;
                pickRoutine1.Slot = task.SourceSlot;
                pickRoutine1.Blade = Hand.Both;
                steps.Enqueue(pickRoutine1);
            }


            if (task.DestStaion != ModuleName.Robot)
            {
                placeRoutine1.Station = task.DestStaion;
                placeRoutine1.Slot = task.DestSlot;
                placeRoutine1.Blade = Hand.Both;
                steps.Enqueue(placeRoutine1);
            }
        }
        ///
        private bool CanUsedBlade2(MoveTask task1, MoveTask task2, MoveTask task3, MoveTask task4, out int handledTask)
        {
            handledTask = 0;


            if (task1 == null || task1.SourceSlot + 3 > 24)
            {
                return false;
            }

            if (task1 != null && task2 != null && task3 != null && task4 != null)
            {
                if ((task2.SourceSlot == task1.SourceSlot + 1) && (task2.DestSlot == task1.DestSlot + 1)
                    && (task3.SourceSlot == task2.SourceSlot + 1) && (task3.DestSlot == task2.DestSlot + 1)
                    && (task4.SourceSlot == task3.SourceSlot + 1) && (task4.DestSlot == task3.DestSlot + 1)
                    )
                {
                    if (CheckEmptySlot(task1))
                    {
                        handledTask = 4;
                        return true;
                    }
                }
            }
            return false;
        }

        private bool _CanUsedBlade2(MoveTask task1, out int handledTask)
        {
            handledTask = 0;

            bool[] bExists = new bool[] { true, false, false, false };


            if (task1.SourceSlot + 3 > 24)
            {
                return false;
            }

            if (CheckEmptySlot(task1, bExists))
            {
                handledTask = 1;
                return true;
            }

            return false;
        }

        private bool _CanUsedBlade2(MoveTask task1, MoveTask task2, out int handledTask)
        {
            handledTask = 0;

            bool[] bExists = new bool[] { true, false, false, false };
            int offset = task2.SourceSlot - task1.SourceSlot;

            if (offset > 3)
            {
                return _CanUsedBlade2(task1, out handledTask);
            }

            if (task2.SourceSlot - task1.SourceSlot == task2.DestSlot - task1.DestSlot)
            {
                bExists[task2.SourceSlot - task1.SourceSlot] = true;

                if (CheckEmptySlot(task1, bExists))
                {
                    handledTask = 2;
                    return true;
                }
            }


            return false;
        }

        private bool _CanUsedBlade2(MoveTask task1, MoveTask task2, MoveTask task3, out int handledTask)
        {
            handledTask = 0;
            bool[] bExists = new bool[] { true, false, false, false };

            int offset = task3.SourceSlot - task1.SourceSlot;
            if (offset > 3)
            {
                return _CanUsedBlade2(task1, task2, out handledTask);
            }

            if (task2.SourceSlot - task1.SourceSlot == task2.DestSlot - task1.DestSlot
                && task3.SourceSlot - task1.SourceSlot == task3.DestSlot - task1.DestSlot)
            {
                bExists[task2.SourceSlot - task1.SourceSlot] = true;
                bExists[task3.SourceSlot - task1.SourceSlot] = true;

                if (CheckEmptySlot(task1, bExists))
                {
                    handledTask = 3;
                    return true;
                }
            }

            return false;
        }
        private bool _CanUsedBlade2(MoveTask task1, MoveTask task2, MoveTask task3, MoveTask task4, out int handledTask)
        {
            handledTask = 0;
            bool[] bExists = new bool[] { true, false, false, false };

            int offset = task4.SourceSlot - task1.SourceSlot;
            if (offset > 3)
            {
                return _CanUsedBlade2(task1, task2, task3, out handledTask);
            }


            if (task2.SourceSlot - task1.SourceSlot == task2.DestSlot - task1.DestSlot
                && task3.SourceSlot - task1.SourceSlot == task3.DestSlot - task1.DestSlot
                && task4.SourceSlot - task1.SourceSlot == task4.DestSlot - task1.DestSlot)
            {
                bExists[task2.SourceSlot - task1.SourceSlot] = true;
                bExists[task3.SourceSlot - task1.SourceSlot] = true;
                bExists[task4.SourceSlot - task1.SourceSlot] = true;

                if (CheckEmptySlot(task1, bExists))
                {
                    handledTask = 4;
                    return true;
                }
            }


            return false;
        }
        /// <summary>
        /// 
        /// </summary>
        /// <param name="objs"></param>
        /// <returns>如果任务全部完成，或者任务失败，返回true</returns>
        public bool Monitor(object[] objs)
        {
            Result ret = Result.RUN;

            if (tasks.Count == 0 && steps.Count == 0 && curStep == null)
            {
                return true;             //completed
            }

            if (curStep != null)
            {
                ret = curStep.Monitor();
            }

            if (ret == Result.DONE || curStep == null)
            {
                if (_pause)
                {
                    return false;       //pause
                }

                if (steps.Count > 0)
                {
                    curStep = steps.Dequeue();
                }
                else
                {
                    newTask();

                    if (steps.Count > 0)
                    {
                        curStep = steps.Dequeue();
                    }
                    else
                        curStep = null;
                }

                if (curStep != null)
                {
                    ret = curStep.Start();
                    if (ret == Result.FAIL)
                    {
                        WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 0, EnumWaferProcessStatus.Failed);
                        WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 1, EnumWaferProcessStatus.Failed);
                        WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 2, EnumWaferProcessStatus.Failed);
                        WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 3, EnumWaferProcessStatus.Failed);
                        WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 4, EnumWaferProcessStatus.Failed);
                        NotifyError("Robot", "Robot Error");
                        return true;               //transition 
                    }
                }
                else
                {
                    return true;    //transition 
                }

                return false;
            }
            else if (ret == Result.FAIL)
            {
                WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 0, EnumWaferProcessStatus.Failed);
                WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 1, EnumWaferProcessStatus.Failed);
                WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 2, EnumWaferProcessStatus.Failed);
                WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 3, EnumWaferProcessStatus.Failed);
                WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 4, EnumWaferProcessStatus.Failed);
                NotifyError("Robot", "Robot Error");
                return true;               //transition                  
            }

            return false;
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="objs"></param>
        /// <returns>如果任务全部完成，或者任务失败，返回true</returns>
        public Result Monitor()
        {
            Result ret = Result.RUN;

            if (tasks.Count == 0 && steps.Count == 0 && curStep == null)
            {
                return Result.DONE;             //completed
            }

            if (curStep != null)
            {
                ret = curStep.Monitor();
            }

            if (ret == Result.DONE || curStep == null)
            {
                if (_pause)
                {
                    return Result.RUN;       //pause
                }

                if (steps.Count > 0)
                {
                    curStep = steps.Dequeue();
                }
                else
                {
                    newTask();

                    if (steps.Count > 0)
                    {
                        curStep = steps.Dequeue();
                    }
                    else
                        curStep = null;
                }

                if (curStep != null)
                {
                    ret = curStep.Start();
                    if (ret == Result.FAIL)
                    {
                        WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 0, EnumWaferProcessStatus.Failed);
                        WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 1, EnumWaferProcessStatus.Failed);
                        WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 2, EnumWaferProcessStatus.Failed);
                        WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 3, EnumWaferProcessStatus.Failed);
                        WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 4, EnumWaferProcessStatus.Failed);
                        NotifyError("Robot", "Robot Error");
                        return Result.FAIL;               //transition 
                    }
                }
                else
                {
                    return Result.DONE;    //transition 
                }

                return Result.RUN;
            }
            else if (ret == Result.FAIL)
            {
                WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 0, EnumWaferProcessStatus.Failed);
                WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 1, EnumWaferProcessStatus.Failed);
                WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 2, EnumWaferProcessStatus.Failed);
                WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 3, EnumWaferProcessStatus.Failed);
                WaferManager.Instance.UpdateWaferProcessStatus(ModuleName.Robot, 4, EnumWaferProcessStatus.Failed);
                NotifyError("Robot", "Robot Error");
                return Result.FAIL;               //transition                  
            }

            return Result.RUN;
        }

        private bool CheckEmptySlot(MoveTask task)
        {
            for (int i = 0; i < 4; i++)
            {
                if (!WaferManager.Instance.CheckWafer(task.SourceStaion, task.SourceSlot + i, WaferStatus.Normal))
                    return false;

                if (!WaferManager.Instance.CheckNoWafer(task.DestStaion, task.DestSlot + i))
                    return false;

            }
            return true;
        }
        private bool CheckEmptySlot(MoveTask task, bool[] exist)
        {
            for (int i = 0; i < 4; i++)
            {
                if (!exist[i])
                {
                    if (!WaferManager.Instance.CheckNoWafer(task.SourceStaion, task.SourceSlot + i))
                        return false;
                }


                if (!WaferManager.Instance.CheckNoWafer(task.DestStaion, task.DestSlot + i))
                    return false;

            }
            return true;
        }
        private void NotifyError(string module, string message)
        {
            if (OnMoveError != null)
            {
                OnMoveError(new MoveErrorArgument(module, message));
            }
        }
    }
}