using Microsoft.AspNetCore.Mvc.Diagnostics;
using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared.Sensor;
using System;
using System.Net;
using System.Net.Sockets;
//using static RobotNet10.RobotApp.Drivers.Lidar.OleiLidarDecode;

namespace RobotNet10.RobotApp.Drivers.Lidar
{
    [Device(DeviceType.Lidar, "Olei", "OleiLidarDriver", "1.0.0")]
    public class OleiLidarDriver : DeviceBase, ILidar
    {
        private readonly object _scanDataLock = new();


        // Cached scan data
        //private LidarMeasurementData? _cachedMeasurementData = null;

        // Device specifications
        private readonly double _minRangeM;
        private readonly double _maxRangeM;
        private readonly double? _angularResolutionRad;
        private readonly double? _scanFrequencyHz;
        private readonly bool _supportsIntensity;
        private readonly double? _accuracyM;
        private readonly int _numberOfPoints;
        private readonly double _startAngleRad;
        private readonly double _endAngleRad;
        private DateTime? _lastScanDataTimestamp;
        private LaserScan? _lastLaserScan;
        private oleiPackage? _pkt;
        Connect oleiConnect = new Connect();
        DecodeLidar oleiDecode = new DecodeLidar();

        DecoderConfig decoderConfig = new DecoderConfig();
        private readonly Lock _dataLock = new();
        private CancellationTokenSource? _updateCts;
        private Task? _updateTask;
        public float maxAngle;
        public float minAngle;
        public UdpClient udp;
        public IPEndPoint check_ip_device;
        bool portOpened;

        //Event
        public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;

        //Cached data
        //private LidarMeasurementData? _currentMeasurementData;
        public OleiLidarDriver(string deviceId, string deviceName, IConfigurationSection connection)
        : base(deviceId, deviceName, DeviceType.Lidar)
        {
            // Đọc cấu hình kết nối (IP và Port)
            string deviceIp = connection.GetValue<string>("DeviceIp") ?? "192.168.254.13";
            int devicePort = connection.GetValue<int?>("DevicePort") ?? 2368;
            string localIp = connection.GetValue<string>("LocalIp") ?? "192.168.254.10";
            
            // Cấu hình Connect object với IP và Port từ config
            oleiConnect.device_IP = deviceIp;
            oleiConnect.device_port = devicePort;
            oleiConnect.local_ip = localIp;
            // Đọc cấu hình
            minAngle = connection.GetValue<float?>("MinAngle") ?? -130.0f;
            maxAngle = connection.GetValue<float?>("MaxAngle") ?? 130.0f;
            _maxRangeM = connection.GetValue<double?>("MaxRangeM") ?? 50.0;
            _minRangeM = connection.GetValue<double?>("MinRangeM") ?? 0.1; // Default 20cm
            _scanFrequencyHz = connection.GetValue<double?>("ScanFrequencyHz") ?? 10.0; // Default 10Hz
            _supportsIntensity = connection.GetValue<bool?>("SupportsIntensity") ?? true;
            _accuracyM = connection.GetValue<double?>("AccuracyM") ?? 0.02; // Default 2cm accuracy

            // Set decoder config from JSON
            decoderConfig.Inverted = connection.GetValue<bool?>("Inverted") ?? false;

            // Apply config to decoder
            oleiDecode.SetConfig(decoderConfig);

            var autoReconnectEnabled = connection.GetValue<bool?>("AutoReconnectEnabled");
            var reconnectDelayMs = connection.GetValue<int?>("ReconnectDelayMs");
            var maxReconnectAttempts = connection.GetValue<int?>("MaxReconnectAttempts");

            if (autoReconnectEnabled.HasValue)
                AutoReconnectEnabled = autoReconnectEnabled.Value;
            else
                AutoReconnectEnabled = false; // Không cần reconnect cho simulation

            if (reconnectDelayMs.HasValue)
                ReconnectDelayMs = reconnectDelayMs.Value;

            if (maxReconnectAttempts.HasValue)
                MaxReconnectAttempts = maxReconnectAttempts.Value;

            // Khởi tạo giá trị properties
            UpdateProperties();
        }

        protected override IEnumerable<PropertyDescription> CreatePropertyDescriptions()
        {
            yield return new PropertyDescription("NumberOfPoints", "Number of Points", "Số điểm scan")
            {
                DataType = "number",
                IsReadOnly = true,
                DisplayOrder = 1,
                Category = "Cấu hình",
                DefaultValue = "2000"
            };

            yield return new PropertyDescription("StartAngle", "Start Angle (deg)", "Góc bắt đầu (độ)")
            {
                DataType = "number",
                IsReadOnly = true,
                DisplayOrder = 2,
                Category = "Cấu hình",
                DefaultValue = "-180"
            };

            yield return new PropertyDescription("EndAngle", "End Angle (deg)", "Góc kết thúc (độ)")
            {
                DataType = "number",
                IsReadOnly = true,
                DisplayOrder = 3,
                Category = "Cấu hình",
                DefaultValue = "180"
            };

            yield return new PropertyDescription("MaxRange", "Max Range (m)", "Tầm quét tối đa (m)")
            {
                DataType = "number",
                IsReadOnly = true,
                DisplayOrder = 4,
                Category = "Cấu hình",
                DefaultValue = "20"
            };

            yield return new PropertyDescription("LastScanTime", "Last Scan Time", "Thời gian scan cuối cùng")
            {
                DataType = "string",
                IsReadOnly = true,
                DisplayOrder = 5,
                Category = "Trạng thái",
                DefaultValue = ""
            };
        }

        protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
        {
            // Mở cổng UDP - kiểm tra kết quả và throw exception nếu fail
            lock (_dataLock)
            {
                portOpened = oleiConnect.openPort();
            }
            await Task.CompletedTask;
        }

        protected override async Task OnConnectAsync(CancellationToken cancellationToken)
        {
            // Bắt đầu vòng lặp cập nhật dữ liệu với tần số 1Hz
            StartUpdateLoop();
            await Task.CompletedTask;
        }

        protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
        {
            // Dừng vòng lặp cập nhật
            StopUpdateLoop();
            await Task.CompletedTask;
        }

        protected override async Task OnResetAsync(CancellationToken cancellationToken)
        {
            // Reset về giá trị mặc định
            lock (_dataLock)
            {
                _lastLaserScan = null;
                _lastScanDataTimestamp = null;
                _pkt = null;
            }
            UpdateProperties();
            await Task.CompletedTask;
        }

        protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
        {
            // Simulation luôn connected
            if (!portOpened)
            {
                return await Task.FromResult(false);
            }
            return await Task.FromResult(true);
        }

        private void StartUpdateLoop()
        {
            lock (_dataLock)
            {
                if (_updateTask != null && !_updateTask.IsCompleted)
                    return;

                _updateCts = new CancellationTokenSource();
                _updateTask = Task.Run(() => UpdateLoopAsync(_updateCts.Token));
            }
        }

        private void StopUpdateLoop()
        {
            lock (_dataLock)
            {
                _updateCts?.Cancel();
                _updateCts?.Dispose();
                _updateCts = null;
                _updateTask = null;
            }
        }

        private async Task UpdateLoopAsync(CancellationToken cancellationToken)
        {
            //_connect.openPort();
            //Console.WriteLine("[INFO] START READ DATA");
            while (!cancellationToken.IsCancellationRequested)
            {
                try
                {
                    // Đọc dữ liệu từ UDP
                    lock (_dataLock)
                    {
                        oleiConnect.readRawdata();

                        if (oleiConnect.GetPacketCount() > 0)
                        {
                            GenerateScanData();
                        }
                    }
                }
                catch (OperationCanceledException)
                {
                    break;
                }
                catch (Exception ex)
                {
                    LastError = ex;
                    OnErrorOccurred(ex, "Update loop error");
                    await Task.Delay(1000, cancellationToken);
                }
            }
        }

        private void GenerateScanData()
        {
            // Dequeue packet an toàn (thread-safe)
            oleiPackage? pkt = oleiConnect.TryDequeuePacket();

            if (pkt == null)
            {
                // Không log để tránh spam khi queue rỗng
                return;
            }

            var timestamp = DateTime.UtcNow;

            // Decode packet
            oleiDecode.PacketCb(pkt);

            // Kiểm tra xem có dữ liệu không
            if (oleiDecode.scanRadAngleInVec.Count == 0 ||
                oleiDecode.scanRadAngleInVec.Count != oleiDecode.scanRangeInVec.Count ||
                oleiDecode.scanRadAngleInVec.Count != oleiDecode.scanIntensityInVec.Count)
            {
                // Không có dữ liệu hoặc dữ liệu không đồng bộ
                return;
            }

            var numberOfPoints = oleiDecode.scanRadAngleInVec.Count;
            var ranges = new double[numberOfPoints];
            var intensities = new double[numberOfPoints];
            // Convert data to arrays (LaserScan format)
            for(int i = 0; i < numberOfPoints; i++)
            {
                if((oleiDecode.scanAngleInVec[i] >= (minAngle + 180)) && (oleiDecode.scanAngleInVec[i] <= (maxAngle + 180)))
                {
                    var distanceM = oleiDecode.scanRangeInVec[i] * 0.001f; // mm to meters
                    var isValid = distanceM >= _minRangeM && distanceM <= _maxRangeM;
                    ranges[i] = isValid ? distanceM : 0.0f;
                    intensities[i] = oleiDecode.scanIntensityInVec[i]; // normalize 0-1
                }
            }
            float _minAngle = (float)(minAngle*Math.PI)/180;
            float _maxAngle = (float)(maxAngle*Math.PI)/180;
            float angleIncrement = numberOfPoints > 1 ?
                (_maxAngle - _minAngle) / (numberOfPoints - 1) : 0.0f;
            float _scanTime = 1.0f / oleiDecode.Frequency;
            float _TimeIncrement = _scanTime / numberOfPoints;

            var laserScan = new LaserScan
            {
                Header = new RobotNet10.Shared.Header
                {
                    Seq = 0,
                    Stamp = timestamp,
                    FrameId = "olei_lidar_frame"
                },
                AngleMin = _minAngle,
                AngleMax = _maxAngle,
                AngleIncrement = angleIncrement,
                TimeIncrement = _TimeIncrement,
                ScanTime = _scanTime, // Convert Hz to seconds
                RangeMin = (float)_minRangeM,
                RangeMax = (float)_maxRangeM,
                Ranges = ranges,
                Intensities = intensities
            };

            // Cache LaserScan và packet
            lock (_dataLock)
            {
                _lastLaserScan = laserScan;
                _lastScanDataTimestamp = timestamp;
                _pkt = pkt;
            }

            // Fire event
            ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(timestamp: timestamp, measurementData: laserScan));

            UpdateProperties();
        }
        private void UpdateProperties()
        {
            lock (_dataLock)
            {
                SetProperty("NumberOfPoints", _numberOfPoints.ToString());
                SetProperty("StartAngle", (_startAngleRad * 180.0 / Math.PI).ToString("F1"));
                SetProperty("EndAngle", (_endAngleRad * 180.0 / Math.PI).ToString("F1"));
                SetProperty("MaxRange", _maxRangeM.ToString("F1"));
                SetProperty("LastScanTime", _lastScanDataTimestamp?.ToString("yyyy-MM-dd HH:mm:ss.fff") ?? "");
            }
        }

       #region ILidar Implementation

        public LaserScan? CurrentMeasurementData
        {
            get { lock (_dataLock) { return _lastLaserScan; } }
        }

        public DateTime? LastScanDataTimestamp
        {
            get { lock (_dataLock) { return _lastScanDataTimestamp; } }
        }

        // ILidar Device Specifications
        public double MinAngleRad => _startAngleRad;
        public double MaxAngleRad => _endAngleRad;
        public double MinRangeM => _minRangeM;
        public double MaxRangeM => _maxRangeM;
        public double? AngularResolutionRad => _angularResolutionRad;
        public double? ScanFrequencyHz => _scanFrequencyHz;
        public double FieldOfViewRad => Math.Abs(_endAngleRad - _startAngleRad);
        public bool SupportsIntensity => _supportsIntensity;
        public double? AccuracyM => _accuracyM;

        #endregion

        protected override void Dispose(bool disposing)
        {
            if (disposing)
            {
                StopUpdateLoop();
            }
            base.Dispose(disposing);
        }
    }


}