using Aurora.Api;
using Aurora.Util.HislogPlus;
using AuroraLaser;
using AuroraLaser.Trumpf;
using Hermes;
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Net;
using System.Runtime.Remoting.Contexts;
using System.Runtime.Remoting.Messaging;
using System.Text;
using System.Threading.Tasks;


namespace AuroraOCT.Trumpf
{
    /// <summary>
    /// Laser에 종속적으로 동작해야해서 제조사 별(컨셉이 다름..) Device를 만들어서 동작하도록 함
    /// TruControl 1000 v4.04 이상 사용 가능
    /// </summary>
    public class OCT : Device
    {
        private Logger _log = new Logger("OCT");
        private Laser _laser;
        private LaserTruFiber _core;

        private OCTConfig _config;

        private AngleTracker _tracker = new AngleTracker();
        public int LightPath { get; set; }
        public delegate double getDouble();
        public getDouble GetAXVelocity, GetAXPosition, GetAYVelocity, GetAYPosition;
        public getDouble GetBXVelocity, GetBXPosition, GetBYVelocity, GetBYPosition;

        public OCT(Laser laser) {
            _laser = laser;
        }

        protected override int DoActivate(IDeviceConfig config)
        {
            _config = config as OCTConfig;
            _core = _laser.getCore() as LaserTruFiber;

            if (_laser.IsActivated && _config.IsMonitoringStart){
                SetMonitoringService(UpdatePosition, _config.UpdateInterval);
                StartMonitoringService();
            }

            return _laser.IsActivated ? E1.OK : E2.ABORTED;
        }

        protected override int DoDeactivate()
        {
            StopMonitoringService();
            return E1.OK;
        }

        private void UpdatePosition()
        {
            double XVel, YVel, XPos, YPos;
            if (LightPath == 0) {
                XVel = GetAXVelocity();
                YVel = GetAYVelocity();
                XPos = GetAXPosition();
                YPos = GetAYPosition();
            } else {
                XVel = GetBXVelocity();
                YVel = GetBYVelocity();
                XPos = -GetBXPosition();
                YPos = -GetBYPosition();
            }

            // X,Y 속도의 스칼라량 (벡터의 크기를 전달)
            var velocityVector = Math.Sqrt(XVel * XVel + YVel * YVel);
            var value = velocityVector < 0.001 ? 0.001 : velocityVector;
            SetPositionX(value);

            // feed direction (각 변화량)
            var result = _tracker.UpdatePosition(XPos, YPos);
            if (result.HasValue) {
                value = result.Value.AngleDeg < 0.001 ? 0.001 : result.Value.AngleDeg;
                SetPositionY(value);
                //Debug.WriteLine($"velocity vector : {velocityVector} / AngleDeg : {result.Value.AngleDeg}");
            }
        }
        public double GetPositionX() {
            var word1 = _core.GetWriteInputWord(_config.XPosIW_2W);
            var word2 = _core.GetWriteInputWord(_config.XPosIW_2W + 1);
            UInt32 raw = ((UInt32)(ushort)word2 << 16) | (UInt32)(ushort)word1;
            return raw / 1000.0;
        }

        public double GetPositionY()
        {
            var word1 = _core.GetWriteInputWord(_config.YPosIW_2W);
            var word2 = _core.GetWriteInputWord(_config.YPosIW_2W + 1);
            UInt32 raw = ((UInt32)(ushort)word2 << 16) | (UInt32)(ushort)word1;
            return raw / 1000.0;
        }
        public void SetInquiry1(bool on) => _core.SetData(csWriteNodeNo.INQUIRY_1, on ? 1 : 0);
        public void SetPositionX(double pos) => _core.SetDWord(_config.XPosIW_2W, (int)(pos * 1000));
        public void SetPositionY(double pos) => _core.SetDWord(_config.YPosIW_2W, (int)(pos * 1000));

        public void SetWorkPieceSerialNo(string SerialNo)
        {
            short[] words = ToWordsLowFirst(SerialNo, 4);
            ulong wordsTolong = WordsToUlong_LswFirst(words);
            _core.SetMultiWord(_config.WpSerialNoIW_4W, 4, wordsTolong);
        }

        public void SetWorkPieceID(string id)
        {
            short[] words = ToWordsLowFirst(id, 4);
            ulong wordsTolong = WordsToUlong_LswFirst(words);
            _core.SetMultiWord(_config.WpIdIW_4W, 4, wordsTolong);
        }

        public void SetOctReady(bool preTrigger, bool inTrigger, bool posGen)
        {
            int CurrentWordValue = 0;
            CurrentWordValue = SetBit(CurrentWordValue, (int)eOctControlInputBit.PreTrigger, preTrigger);
            CurrentWordValue = SetBit(CurrentWordValue, (int)eOctControlInputBit.InTrigger, inTrigger);
            CurrentWordValue = SetBit(CurrentWordValue, (int)eOctControlInputBit.PosGen, posGen);

            try {
                _core.SetData(_config.VLCKcontrolIW, CurrentWordValue);
            } catch (Exception ex) {
                _log.D($"SetOctReady Fails; {ex.Message}");
            }
        }

        public void SetPreTrigger(bool on) => SetOctOutputBit(_config.VLCKcontrolIW, (int)eOctControlInputBit.PreTrigger, on);
        public bool IsPreTrigger() => GetOctOutputBit(_config.VLCKcontrolIW, (int)eOctControlInputBit.PreTrigger);

        public void SetInTrigger(bool on) => SetOctOutputBit(_config.VLCKcontrolIW, (int)eOctControlInputBit.InTrigger, on);
        public bool IsInTrigger() => GetOctOutputBit(_config.VLCKcontrolIW, (int)eOctControlInputBit.InTrigger);

        public void SetPosGen(bool on) => SetOctOutputBit(_config.VLCKcontrolIW, (int)eOctControlInputBit.PosGen, on);
        public bool IsPosGen() => GetOctOutputBit(_config.VLCKcontrolIW, (int)eOctControlInputBit.PosGen);

        public void SetLightPath(int no) => _core.SetData(csWriteNodeNo.LIGHT_PATH_NO, no);

        public bool IsSensoricReady() => GetOctOutputBit(eOctControlOutputBit.SensoricReady);
        public bool IsMeasureActive() => GetOctOutputBit(eOctControlOutputBit.MeasureActive);
        public bool IsSeamOk() => GetOctOutputBit(eOctControlOutputBit.SeamOk);
        public bool IsSeamNok() => GetOctOutputBit(eOctControlOutputBit.SeamNok);
        public bool IsProcedureFaultSeam() => GetOctOutputBit(eOctControlOutputBit.ProcedureFaultSeam);
        public bool IsPartOk() => GetOctOutputBit(eOctControlOutputBit.PartOk);
        public bool IsPartNok() => GetOctOutputBit(eOctControlOutputBit.PartNok);
        public bool IsProcedureFaultPart() => GetOctOutputBit(eOctControlOutputBit.ProcedureFaultPart);

        public (eOctControlOutputBit result, bool success) GetSeamResult()
        {
            (eOctControlOutputBit result, bool success) res;
            bool[] results = new bool[3];
            results[0] = IsSeamOk();
            results[1] = IsSeamNok();
            results[2] = IsProcedureFaultSeam();

            if(results[0])
                res.result = eOctControlOutputBit.SeamOk;
            else if (results[1])
                res.result = eOctControlOutputBit.SeamNok;
            else
                res.result = eOctControlOutputBit.ProcedureFaultSeam;

            res.success = results.Select(ok => ok ? 1 : 0).Sum() > 0;
            return res;
        }

        public (eOctControlOutputBit result, bool success) GetPartResult()
        {
            (eOctControlOutputBit result, bool success) res;
            bool[] results = new bool[3];
            results[0] = IsPartOk();
            results[1] = IsPartNok();
            results[2] = IsProcedureFaultPart();

            if (results[0])
                res.result = eOctControlOutputBit.PartOk;
            else if (results[1])
                res.result = eOctControlOutputBit.PartNok;
            else
                res.result = eOctControlOutputBit.ProcedureFaultPart;

            res.success = results.Select(ok => ok ? 1 : 0).Sum() > 0;
            return res;
        }

        #region OCT 관련 비트 매핑
        public enum eOctControlOutputBit : int
        {
            SensoricReady = 0,
            MeasureActive = 1,
            SeamOk = 2,
            SeamNok = 3,
            PartOk = 4,
            PartNok = 5,
            ProcedureFaultSeam = 8,
            ProcedureFaultPart = 9,
        }

        public enum eOctControlInputBit
        {
            PreTrigger =0,
            InTrigger =1,
            PosGen = 15
        }

        private bool GetOctOutputBit(eOctControlOutputBit no) {
            if (!Enum.IsDefined(typeof(eOctControlOutputBit), no)) return false;

            int temp = _core.ReadNode[_config.OutputWord].n16Ret;

            return (temp & (1 << (int)no)) != 0;
        }

        private int SetBit(int value, int bit, bool flag)
        {
            if (flag)
                return value | (1 << (int)bit);
            else
                return value & ~(1 << (int)bit);
        }

        private bool SetOctOutputBit(int wordNo, int bitNo, bool on)
        {
            var CurrentWordValue = _core.GetWriteInputWord(wordNo);
            CurrentWordValue = SetBit(CurrentWordValue, bitNo, on);

            try {
                    _core.SetData(wordNo, CurrentWordValue);
                return true;
            } catch (Exception ex) {
                _log.D($"SetOctInputBits Fails; {ex.Message}");
                return false;
            }
        }

        private bool GetOctOutputBit(int wordNo, int bitNo)
        {
            var CurrentWordValue = _core.GetWriteInputWord(wordNo);
            return (CurrentWordValue & (1 << (int)bitNo)) != 0;
        }
        //////// 시퀸스 관련 정리
        /// TruFiber의 LaserOn으로 Laser Ready 갈 수 있음.
        /// !! 주의. Program No는 0일 수 있으므로, 유동적으로 변경 가능하도록 수정하여야 할 듯 함.
        /// 이 후에는 우리가 시퀸스에서 제어
        /// OCT -> PC : PC가 값 읽어야 함. PC -> OCT : PC에서 값을 써주어야 함.
        /// 0. OCT -> PC : PC는 LASER_READY 확인
        /// 1. PC -> OCT : PSTART_STATICAL ON
        /// 2. OCT -> PC : PC는 LASER_READY OFF 확인 & SENSORIC_READY ON 확인
        /// 3. PC -> OCT : XPOS,YPOS,POS_GEN 값 변경
        /// 4. OCT -> PC : Reach In Pos 어떻게 확인하지 ?
        /// 5. PC -> OCT : PRE_TRIGGER와 IN_TRIGGER ON. 동시에 킬 것 !!
        /// ##### 레이저 출사 시작
        /// 6. OCT -> PC : MEASUREMENT_ACTIVE ON 확인 (OCT 문제 없는 상태임.)
        /// ##### 레이저 출사 종료
        /// 7. OCT -> PC : MEASUREMENT_ACTIVE OFF 확인 (OCT 측정 종료된 상태임.)
        /// 8. PC -> OCT : (5)에서 써준 값 OFF
        /// 9. OCT -> PC : SEAM_OK or NON_OK or PROCEDURE_FAULT_SEAM ON 확인
        /// 10. OCT -> PC : PROG_COMPLETED ON 확인
        /// 11. PC -> OCT : PSTART_STATICAL OFF
        /// 12. OCT -> PC : Part 관련 값 확인
        #endregion

        public short[] ToWordsLowFirst(string text, int wordCount = 4)
        {
            // 4워드면 최대 8바이트(문자 8개)까지
            int maxBytes = wordCount * 2;
            byte[] bytes = Encoding.ASCII.GetBytes(text);

            if (bytes.Length > maxBytes)
                return null;

            var result = new short[wordCount];

            int bi = 0;
            for (int wi = 0; wi < wordCount; wi++) {
                byte hi = (bi < bytes.Length) ? bytes[bi++] : (byte)0x00;
                byte lo = (bi < bytes.Length) ? bytes[bi++] : (byte)0x00;

                ushort word = (ushort)((hi << 8) | lo);      // 예: 'W','P' => 0x5750
                result[wi] = unchecked((short)word);         // short에 담아 리턴
            }

            return result;
        }

        public ulong WordsToUlong_LswFirst(short[] words)
        {
            if (words == null)
                return 0;

            if (words.Length != 4)
                return 0;

            ulong v = 0;
            for (int i = 0; i < 4; i++) {
                v |= ((ulong)(ushort)words[i]) << (16 * i);
            }
            return v;
        }

        public class AngleTracker
        {
            private double? _prevAngleDeg = null;
            private (double X, double Y)? _prevPosition = null;

            /// 현재 위치를 기준으로 진행 각도(degree)와 이전 위치 대비 각도 변화량을 계산
            public (double AngleDeg, double DeltaAngleDeg)? UpdatePosition(double currX, double currY)
            {
                if (!_prevPosition.HasValue) {
                    _prevPosition = (currX, currY);
                    return null;
                }

                var (prevX, prevY) = _prevPosition.Value;

                double dx = currX - prevX;
                double dy = currY - prevY;

                // 각도 계산
                double angleRad = Math.Atan2(dy, dx);
                double angleDeg = angleRad * (180.0 / Math.PI);

                // 정규화
                double angleDeg360 = (angleDeg + 360) % 360;
                double deltaAngle = 0;

                if (_prevAngleDeg.HasValue) {
                    deltaAngle = angleDeg - _prevAngleDeg.Value;

                    // -180 ~ 180 사이로 정규화
                    while (deltaAngle > 180) deltaAngle -= 360;
                    while (deltaAngle < -180) deltaAngle += 360;
                }

                // 상태 업데이트
                _prevPosition = (currX, currY);
                _prevAngleDeg = angleDeg;

                return (angleDeg360, deltaAngle);
            }
        }
    }
}