LGES-2170-ACW_CCW-Welder/Machines/ILSOS_CCW_H52A/Device/PLC/PLCContoller.Config.cs

using Aurora;
using Aurora.Api;
using AuroraPLC;
using Hermes;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using WeldingCommon.Devices;
using static System.Windows.Forms.VisualStyles.VisualStyleElement.Rebar;

namespace ILSOS_CCW_H52A.Device
{
    internal partial class PLCControllerCCWH52A : PLCController
    {
        public PLCControllerCCWH52A(string deviceId, ePLCNetType plcNetType, PLCMMapFileType fileType, IAuroraEnvironment aurora) : base(deviceId, plcNetType, fileType, aurora)
        {
        }

        protected override int DoActivate(IDeviceConfig config)
        {
            var ret = base.DoActivate(config);
            if (ret == E1.OK && IsSimul) {
                //WriteBit((int)eBitInput.PLC_AUTO_STATUS, true, ePLCDevice.B, false);
                //WriteBit((int)eBitInput.PLC_AUTO_REQ, true, ePLCDevice.B, false);
                SetMonitoringService(EquipmentSimulator, 1);
            }
            return ret;
        }

        protected override int DoDeactivate()
        {
            StopMonitoringService();

            var res = base.DoDeactivate();

            if (res != E1.OK) {
                Log.E("Deactivate is failed");
                return res;
            }

            return res;
        }

        // -----------------------------
        // Simulation state
        // -----------------------------
        private const int PLC_CURRENT_POS_1_ADDR = 4;      // 기존 (mod 0~360)

        private readonly Random _rand = new Random();

        private readonly Stopwatch _sw = Stopwatch.StartNew();
        private long _lastMs = -1;

        private const int RotaryJigCount = 16;
        private const double JigPerMsec = 300;
        private const double DegreePerRev = 360.0;
        private const double OneCycleMsec = JigPerMsec * RotaryJigCount;
        private const double DegreePerMsec = DegreePerRev / OneCycleMsec;
        private const double DegreePerJig = DegreePerRev / RotaryJigCount; // 22.5
        private const double HalfDegreePerJig = DegreePerJig / 2.0;        // 11.25

        // PLC_CURRENT_POS_1 = degree * 100000
        private const double PlcAngleScale = 100_000.0;

        // 시퀀스가 "다음 지그"를 미리 읽으므로 안전하게 2칸 선행(권장)
        private const int LeadSectors = 2;

        // Motor position
        private readonly object _absLock = new object();
        public double GetSimAbsDeg()
        {
            long value = System.Threading.Interlocked.Read(ref _angleAbsDeg);
            return value / 100000.0;
        }
        private long _angleAbsDeg = 0; // 누적 각도(계속 증가)

        private double _angleModDeg = 0.0; // 0~360 랩핑 각도

        // PLC USE_WELD_JIG bits
        private readonly bool[] _canExist = new bool[RotaryJigCount];
        private readonly bool[] _decided = new bool[RotaryJigCount]; // 이번 회전에서 해당 섹터가 결정됐는지
        private int _prevSector = -1;

        private static void FillBool(bool[] arr, bool value)
        {
            if (arr == null)
                return;
            for (int i = 0; i < arr.Length; i++)
                arr[i] = value;
        }

        private static int Mod(int x, int m)
        {
            int r = x % m;
            return (r < 0) ? (r + m) : r;
        }

        private int GetSectorFromAngle(double angleDeg)
        {
            // 시퀀스의 (angle + half)/per와 동일한 기준으로 섹터 결정
            return (int)((angleDeg + HalfDegreePerJig) / DegreePerJig) % RotaryJigCount;
        }

        private void OnSectorChanged(int prev, int curr)
        {
            // 첫 진입
            if (prev < 0) {
                // 처음 시작 시에는 전부 OFF 상태에서 시작
                FillBool(_canExist, false);
                FillBool(_decided, false);
            } else {
                // 랩(15 -> 0) 감지: 새 바퀴 시작으로 보고 "지나간 영역 OFF"를 강하게 반영
                if (prev == RotaryJigCount - 1 && curr == 0) {
                    FillBool(_canExist, false);
                    FillBool(_decided, false);
                } else {
                    // 지나간 섹터(prev)는 OFF 처리
                    _canExist[prev] = false;
                    _decided[prev] = false; // 다음 회전에서 다시 결정되도록
                }
            }

            // 시퀀스는 "다음 지그"를 읽으므로 curr+1부터 선행 결정
            for (int k = 1; k <= LeadSectors; k++) {
                int s = Mod(curr + k, RotaryJigCount);

                if (!_decided[s]) {
                    // 7/8 확률 ON (0~7 중 0만 OFF로 만들려면 >0)
                    bool isCanExist = true;// (_rand.Next(0, 8) > 0);
                    _canExist[s] = isCanExist;
                    _decided[s] = true;
                }
            }

            // 현재 섹터(curr)도 켜고 싶다면 여기서 결정 가능
        }

        private void EquipmentSimulator()
        {
            if (!ReadBit((int)eBitOutput.PC_AUTO_STATUS, ePLCDevice.B, false).data)
                return;

            long now = _sw.ElapsedMilliseconds;
            if (_lastMs < 0)
                _lastMs = now;

            long dtMs = now - _lastMs;
            if (dtMs < 0)
                dtMs = 0;

            if (dtMs > 100)
                dtMs = 100;

            _lastMs = now;

            // abs 누적각 (시뮬 전용)
            long deltaScaled = (long)Math.Round(DegreePerMsec * dtMs * 100000.0);
            System.Threading.Interlocked.Add(ref _angleAbsDeg, deltaScaled);

            double absDeg = GetSimAbsDeg();
            _angleModDeg = absDeg % DegreePerRev;
            if (_angleModDeg < 0)
                _angleModDeg += DegreePerRev;

            // mod 각도
            int plcAngleMod = (int)Math.Round(_angleModDeg * PlcAngleScale);
            WriteDWord(PLC_CURRENT_POS_1_ADDR, plcAngleMod, ePLCDevice.W, false);

            // 섹터 변경/비트 처리 로직은 기존 그대로
            int sectorNow = GetSectorFromAngle(_angleModDeg);
            if (sectorNow != _prevSector) {
                OnSectorChanged(_prevSector, sectorNow);
                _prevSector = sectorNow;
            }

            WriteBitBlock((int)eBitInput.PLC_USE_WELD_JIG_1, RotaryJigCount, _canExist, ePLCDevice.B, false);
        }

        public void ResetSimulator()
        {
            _lastMs = -1;
            System.Threading.Interlocked.Exchange(ref _angleAbsDeg, 0);
            _angleModDeg = 0.0;

            FillBool(_canExist, false);
            FillBool(_decided, false);
            _prevSector = -1;

            // 각도 초기화 반영
            WriteDWord(PLC_CURRENT_POS_1_ADDR, 0, ePLCDevice.W, false);

            // 지그 비트 초기화 반영
            WriteBitBlock((int)eBitInput.PLC_USE_WELD_JIG_1, RotaryJigCount, _canExist, ePLCDevice.B, false);
        }
    }
}