using ACS.SPiiPlusNET;
using Aurora;
using Aurora.Api;
using Aurora.Api.Devices;
using Aurora.View;
using AuroraACS;
using AuroraHeightSensor;
using AuroraLaser;
using AuroraLaser.Trumpf;
using AuroraMassFlowMeter;
using AuroraMotionBoard.Ajin;
using AuroraOCT.Trumpf;
using Hermes;
using ILSOS_PrismaticGun.Alarm;
using ILSOS_PrismaticGun.Device;
using ILSOS_PrismaticGun.Device.ACS;
using ILSOS_PrismaticGun.Process;
using ILSOS_PrismaticGun.Recipes;
using ILSOS_PrismaticGun.Sequences.SubSequence.StageA;
using ILSOS_PrismaticGun.Sequences.SubSequence.StageB;
using ILSOS_PrismaticGun.Settings;
using ILSOS_PrismaticGun.Tools;
using ILSOS_PrismaticGun.Views.Recipe;
using OpenCvSharp.Dnn;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Threading;
using System.Windows;
using VisionCommon;
using WeldingCommon;
using WeldingCommon.Alarm;
using WeldingCommon.Data;
using WeldingCommon.Sequences;
using WeldingCommon.Settings;
using static AuroraOCT.Trumpf.OCT;
using static WeldingCommon.Devices.PLCController;


namespace ILSOS_PrismaticGun.Sequences
{
    class SeqMotorInit
    {
        static readonly Logger Log = new Logger("MotorInitSequence");
        const int SEQ_TIMEOUT = 120 * 1000;
        const int RESET_TIMEOUT = 10 * 1000;

        const int MOTION_TIMEOUT = 10 * 1000;
        const int MOTION_INTERVAL = 20;

        const string SEQ_ENTER = "SEQ_ENTER";
        const string SEQ_CHECK_INIT_CONDITION = "Check Init Condition";
        const string SEQ_PREPARE_MOTION = "Prepare Motion - JigUp";
        const string SEQ_RE_SCAN = "ReScan";
        const string SEQ_INIT_Z_AXIS = "Init Z Axis";
        const string SEQ_ACS_ALARM_RESET = "ACS Alarm Reset";
        const string SEQ_INIT_ACS_AXIS = "Init ACS X,Y Axis";
        const string SEQ_MOTION_FORK = "Fork Safety Position Move";
        const string SEQ_MOVE_SAFETY_POS_STAGE_A = "Move Safety Pos Stage A";
        const string SEQ_MOVE_SAFETY_POS_STAGE_B = "Move Safety Pos Stage B";
        const string SEQ_MOTION_JOIN = "Join Safety Position Move";
        const string SEQ_INIT_COMPLETE = "Init Complete";

        private static PLCControllerPrismaticGun _Plc;
        private static PrismaticGunACS _acs;
        private static IPrismaticGunACS _motion;
        private static PrismaticGunACSIO _io;

        private static AJINMotor _StageAZ;
        private static AJINMotor _StageBZ;

        private static AJINDevice _Ajin;


        public static SequenceBuilder Builder()
        {
            SequenceBuilder sb = new SequenceBuilder(1000, true);

            sb.AddAction(SEQ_ENTER, Sequence_Enter);
            sb.AddAction(SEQ_CHECK_INIT_CONDITION, CheckInitCondition);
            sb.AddAction(SEQ_PREPARE_MOTION, PrepareMotion);
            sb.AddAction(SEQ_RE_SCAN, AjinRescan);
            sb.AddAction(SEQ_INIT_Z_AXIS, ZAxisInit);
            sb.AddAction(SEQ_ACS_ALARM_RESET, ACSAlarmReset);
            sb.AddAction(SEQ_INIT_ACS_AXIS, ACSAxisInit);
            sb.AddFork(SEQ_MOTION_FORK);
            sb.AddSubsequence(SEQ_MOVE_SAFETY_POS_STAGE_A, SeqTeachingPosMoveStageA.Builder());
            sb.AddSubsequence(SEQ_MOVE_SAFETY_POS_STAGE_B, SeqTeachingPosMoveStageB.Builder());
            sb.AddFork(SEQ_MOTION_JOIN);
            sb.AddAction(SEQ_INIT_COMPLETE, InitComplete);

            sb.OnStopped(OnStopped);
            sb.OnForceError(OnForceError);

            sb.Flows(sb.Init > sb[SEQ_ENTER] > sb[SEQ_CHECK_INIT_CONDITION] > sb[SEQ_PREPARE_MOTION] >
                sb[SEQ_RE_SCAN] > sb[SEQ_INIT_Z_AXIS] > sb[SEQ_ACS_ALARM_RESET] > sb[SEQ_INIT_ACS_AXIS] > sb[SEQ_MOTION_FORK]);
            sb.Flows(sb[SEQ_MOTION_FORK] > sb[SEQ_MOVE_SAFETY_POS_STAGE_A] > sb[SEQ_MOTION_JOIN]);
            sb.Flows(sb[SEQ_MOTION_FORK] > sb[SEQ_MOVE_SAFETY_POS_STAGE_B] > sb[SEQ_MOTION_JOIN]);
            sb.Flows(sb[SEQ_MOTION_JOIN] > sb[SEQ_INIT_COMPLETE] > sb.Final);
            return sb;
        }

        private static SequenceActionResult Sequence_Enter(SequenceContext context, string id, SequenceActionResult param)
        {
            if (_motion is null)
                _motion = _acs = context.Devices[DeviceIds.ACS] as PrismaticGunACS;

            if (_io is null)
                _io = context.Devices[DeviceIds.IODevice] as PrismaticGunACSIO;

            if (_Ajin is null)
                _Ajin = context.Devices[CommonDeviceIds.AJIN] as AJINDevice;

            if (_StageAZ is null)
                _StageAZ = context.Devices[MotorIds.ALineZ] as AJINMotor;

            if (_StageBZ is null)
                _StageBZ = context.Devices[MotorIds.BLineZ] as AJINMotor;

            if (_Plc is null)
                _Plc = context.Devices[DeviceIds.PLCDevice] as PLCControllerPrismaticGun;

            return SequenceAction.Ok();
        }

        private static SequenceActionResult CheckInitCondition(SequenceContext context, string id, SequenceActionResult param)
        {
            if (!((_motion as PrismaticGunACS).IsActivated)) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_A_DEACTIVATED_DISCONNECTED);
                context.PutValue<string>("AlarmMessage",
                    $"[ACS activated({(_motion as PrismaticGunACS).IsActivated})]");
                return null;
            }

            if (!(_io.IsActivated && _io.IsConnected())) {
                Log.E();
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.DEVICE_ACSIO_DEACTIVATED_DISCONNECTED);
                context.PutValue<string>("AlarmMessage",
                    $"[IO activated({_io.IsActivated}) / connected({_io.IsConnected()})]");
                return null;
            }

            if (!(_Plc.IsActivated && _Plc.IsConnected)) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.DEVICE_PLC_DEACTIVATED_DISCONNECTED);
                context.PutValue<string>("AlarmMessage",
                    $"[PLC activated({_Plc.IsActivated}) / connected({_Plc.IsConnected})]");
                return null;
            }

            if(!_Plc.ReadBit(PLCControllerPrismaticGun.eBitInput.PLC_STAGE_A_LOAD_ENABLE).data) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_A_PLC_ENABLE_TIMEOUT);
                context.PutValue<string>("AlarmMessage",
                    $"Stage A Load Enable이 꺼져있습니다.");
                return null;
            }

            if (!_Plc.ReadBit(PLCControllerPrismaticGun.eBitInput.PLC_STAGE_B_LOAD_ENABLE).data) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_B_PLC_ENABLE_TIMEOUT);
                context.PutValue<string>("AlarmMessage",
                    $"Stage B Load Enable이 꺼져있습니다.");
                return null;
            }

            var InbufferPalletExist = _Plc.ExistPalletInBuffer(eStage.A) || _Plc.ExistRealPalletInBuffer(eStage.A);
            if (InbufferPalletExist) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_A_MISMACHING_CELL_EXIST_DATA);
                context.PutValue<string>("AlarmMessage",
                    $"Stage A In-buffer에 데이터({_Plc.ExistPalletInBuffer(eStage.A)}) 또는 실물({_Plc.ExistRealPalletInBuffer(eStage.A)})이 있습니다. 실물과 데이터를 지운 후 진행하세요");
                return null;
            }

            InbufferPalletExist = _Plc.ExistPalletInBuffer(eStage.B) || _Plc.ExistRealPalletInBuffer(eStage.B);
            if (InbufferPalletExist) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_B_MISMACHING_CELL_EXIST_DATA);
                context.PutValue<string>("AlarmMessage",
                    $"Stage B In-buffer에 데이터({_Plc.ExistPalletInBuffer(eStage.B)}) 또는 실물({_Plc.ExistRealPalletInBuffer(eStage.B)})이 있습니다. 실물과 데이터를 지운 후 진행하세요");
                return null;
            }

            return SequenceAction.Ok();
        }

        private static SequenceActionResult PrepareMotion(SequenceContext context, string id, SequenceActionResult param)
        {
            if (!_io.CheckJigUp(eStage.A)) {
                Log.V($"Stage A Jig Up Start");
                _io.JigUp(eStage.A);

                var res = context.RetryUntilSuccess(() => _io.CheckJigUp(eStage.A), MOTION_TIMEOUT, MOTION_INTERVAL);
                if (res == E2.TIMEOUT) {
                    context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_A_JIG_UP_TIMEOUT);
                    context.PutValue<string>("AlarmMessage", "Jig Up Check Timeout!");
                    return null;
                } else if (res == E2.CANCELLED) {
                    context.PutValue<int>("AlarmCode", 0);
                    context.PutValue<string>("AlarmMessage", "Jig Up Check Cancel!");
                    return null;
                }
                Log.V($"Stage A Jig Up Complete");
            }

            if (!_io.CheckJigUp(eStage.B)) {
                Log.V($"Stage B Jig Up Start");
                _io.JigUp(eStage.B);

                var res = context.RetryUntilSuccess(() => _io.CheckJigUp(eStage.B), MOTION_TIMEOUT, MOTION_INTERVAL);
                if (res == E2.TIMEOUT) {
                    context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_B_JIG_UP_TIMEOUT);
                    context.PutValue<string>("AlarmMessage", "Jig Up Check Timeout!");
                    return null;
                } else if (res == E2.CANCELLED) {
                    context.PutValue<int>("AlarmCode", 0);
                    context.PutValue<string>("AlarmMessage", "Jig Up Check Cancel!");
                    return null;
                }
                Log.V($"Stage B Jig Up Complete");
            }

            return SequenceAction.Ok();
        }

        private static SequenceActionResult AjinRescan(SequenceContext context, string id, SequenceActionResult param)
        {
            Log.V($"Ajin ReScan Start");
            if(_Ajin.CheckDeviceLive()){
                Log.I("Ajin Device HomeComplete Check OK - Rescan Skip");
                return SequenceAction.Ok();
            }

            if (!_Ajin.Rescan()) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.DEVICE_MOTOR_DEACTIVATED);
                context.PutValue<string>("AlarmMessage", "Rescan Command Fail");
                return null;
            }

            if (!_Ajin.ReInitialize()) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.DEVICE_MOTOR_DEACTIVATED);
                context.PutValue<string>("AlarmMessage", "ReInitialize Fail");
                return null;
            }

            if (_Ajin.LoadMotFile() != E1.OK) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.DEVICE_MOTOR_DEACTIVATED);
                context.PutValue<string>("AlarmMessage", ".mot File Load Fail");
                return null;
            }
            var res = context.RetryUntilSuccess(() => _Ajin.CheckDeviceLive(), SEQ_TIMEOUT, MOTION_INTERVAL);
            if (res == E2.TIMEOUT) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.DEVICE_MOTOR_DEACTIVATED);
                context.PutValue<string>("AlarmMessage", "[Ajin ReScan Check Fail Timeout]");
                return null;
            } else if (res == E2.CANCELLED) {
                context.PutValue<int>("AlarmCode", 0);
                context.PutValue<string>("AlarmMessage", "[Ajin ReScan Check Fail Cancel]");
                return null;
            }

            Log.V($"Ajin ReScan End");

            return SequenceAction.Ok();
        }

        private static SequenceActionResult ZAxisInit(SequenceContext context, string id, SequenceActionResult param)
        {
            Log.V($"Ajin Z Axis Home Start");

            if(!_StageAZ.IsServoOn())
                _StageAZ.ServoOn(true);

            if (!_StageBZ.IsServoOn())
                _StageBZ.ServoOn(true);

            Thread.Sleep(300);

            if(!_StageAZ.IsHomeCompleted())
                _StageAZ.DoHome();

            if (!_StageBZ.IsHomeCompleted())
                _StageBZ.DoHome();

            var res = context.RetryUntilSuccess(() => _StageAZ.IsHomeCompleted(), SEQ_TIMEOUT, MOTION_INTERVAL);
            if (res == E2.TIMEOUT) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_A_Z_MOVE_TIMEOUT);
                context.PutValue<string>("AlarmMessage", "Ajin StageA Z Axis Home Timeout");
                return null;
            } else if (res == E2.CANCELLED) {
                context.PutValue<int>("AlarmCode", 0);
                context.PutValue<string>("AlarmMessage", "Ajin StageA Z Axis Home Cancel");
                return null;
            }

            res = context.RetryUntilSuccess(() => _StageBZ.IsHomeCompleted(), SEQ_TIMEOUT, MOTION_INTERVAL);
            if (res == E2.TIMEOUT) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_B_Z_MOVE_TIMEOUT);
                context.PutValue<string>("AlarmMessage", "Ajin StageB Z Axis Home Timeout");
                return null;
            } else if (res == E2.CANCELLED) {
                context.PutValue<int>("AlarmCode", 0);
                context.PutValue<string>("AlarmMessage", "Ajin StageB Z Axis Home Cancel");
                return null;
            }
            Log.V($"Ajin Z Axis Home End");

            _StageAZ.Move(0.0, AuroraMotionBoard.Ajin.Speed.L, TargetPosType.ABS);
            Log.V($"Move Z Position : 0.0");

            _StageBZ.Move(0.0, AuroraMotionBoard.Ajin.Speed.L, TargetPosType.ABS);
            Log.V($"Move Z Position : 0.0");

            res = context.RetryUntilSuccess(() => _StageAZ.IsReachedTargetPosition(0.0), MOTION_TIMEOUT, MOTION_INTERVAL, true);
            if (res == E2.TIMEOUT) {
                Log.FuncE($"Z-Axis({Enum.GetName(typeof(eTeachingPosition), eTeachingPosition.SafetyPosition)}) Move Timeout!");
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_A_Z_MOVE_TIMEOUT);
                return null;
            } else if (res == E2.CANCELLED) {
                Log.FuncE($"Z-Axis({Enum.GetName(typeof(eTeachingPosition), eTeachingPosition.SafetyPosition)}) Move Cancel!");
                context.PutValue<int>("AlarmCode", 0);
                return null;
            }

            res = context.RetryUntilSuccess(() => _StageBZ.IsReachedTargetPosition(0.0), MOTION_TIMEOUT, MOTION_INTERVAL, true);
            if (res == E2.TIMEOUT) {
                Log.FuncE($"Z-Axis(0.0) Move Timeout!");
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.STAGE_B_Z_MOVE_TIMEOUT);
                return null;
            } else if (res == E2.CANCELLED) {
                Log.FuncE($"Z-Axis(0.0) Move Cancel!");
                context.PutValue<int>("AlarmCode", 0);
                return null;
            }
            Log.V($"Move Z Position Complete : 0.0");

            return SequenceAction.Ok();
        }

        private static SequenceActionResult ACSAlarmReset(SequenceContext context, string id, SequenceActionResult param)
        {
            Log.V("ACS Alarm Reset Start");
            _motion.ResetAlarm();
            Thread.Sleep(1000);

            var res = context.RetryUntilSuccess(() => !_acs.HasHeavyAlarm & !_acs.HasLightAlarm, RESET_TIMEOUT, MOTION_INTERVAL);
            if (res == E2.TIMEOUT) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.ACS_AXIS_INITIALIZE_FAIL);
                context.PutValue<string>("AlarmMessage", "ACS Alarm Reset Fail");
                return null;
            } else if (res == E2.CANCELLED) {
                context.PutValue<int>("AlarmCode", 0);
                context.PutValue<string>("AlarmMessage", "ACS Alarm Reset Cancel");
                return null;
            }
            Log.V("ACS Alarm Reset End");

            return SequenceAction.Ok();
        }

        private static SequenceActionResult ACSAxisInit(SequenceContext context, string id, SequenceActionResult param)
        {
            Log.V($"ACS Init Start");
            _motion.AllAxisHomeStart();
            Thread.Sleep(1000);

            var res = context.RetryUntilSuccess(() => _motion.CheckAllAxisHome(), SEQ_TIMEOUT, MOTION_INTERVAL);
            if (res == E2.TIMEOUT) {
                context.PutValue<int>("AlarmCode", AlarmCode_PrismaticGun.ACS_AXIS_INITIALIZE_FAIL);
                context.PutValue<string>("AlarmMessage", "ACS Axis Home Timeout");
                return null;
            } else if (res == E2.CANCELLED) {
                context.PutValue<int>("AlarmCode", 0);
                context.PutValue<string>("AlarmMessage", "ACS Axis Home Cancel");
                return null;
            }

            Log.V($"ACS Init End");
            return SequenceAction.Ok(eTeachingPosition.SafetyPosition);
        }

        private static SequenceActionResult InitComplete(SequenceContext context, string id, SequenceActionResult param)
        {
            MessageUtil.ShowSuccessToast("Initialize Complete", Log);
            return SequenceAction.Ok();
        }

        private static void OnStopped(SequenceContext context)
        {
            var AlarmCode = context.GetValue<int>("AlarmCode");
            if (AlarmCode > 0)
                Log.E(context.AlarmAccured(AlarmCode));

            _motion.AllAxisHomeStop();
            _StageAZ.Stop();
            _StageBZ.Stop();

            return;
        }

        private static void OnForceError(SequenceContext context, string arg2)
        {
            var AlarmCode = context.GetValue<int>("AlarmCode");
            if (AlarmCode > 0)
                Log.E(context.AlarmAccured(AlarmCode));

            var message = context.GetValue<string>("AlarmMessage");
            Application.Current.Dispatcher.Invoke(() => { MessageUtil.CreateAndShowMessageDialog("Initialize", message); });

            _motion.AllAxisHomeStop();
            _StageAZ.Stop();
            _StageBZ.Stop();

            return;
        }
    }
}