using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared.Sensor;

namespace RobotNet10.RobotApp.Drivers.Lidar;

/// <summary>
/// Olei 2D LiDAR driver (LR-1BS5 / olelidar ROS protocol, 1240-byte UDP packets).
/// </summary>
[Device(DeviceType.Lidar, "Olei", "OleiLidarDriver", "1.0.0")]
public class OleiLidarDriver : DeviceBase, ILidar
{
    private readonly Connect _connect = new();
    private readonly DecodeLidar _decode = new();

    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 double _startAngleRad;
    private readonly double _endAngleRad;
    private readonly string _frameId;
    private readonly int _angleMinHundredths;
    private readonly int _angleMaxHundredths;
    private readonly int _poly;

    private DateTime? _lastScanDataTimestamp;
    private LaserScan? _lastLaserScan;
    private bool _portOpened;

    private readonly Lock _dataLock = new();
    private CancellationTokenSource? _updateCts;
    private Task? _updateTask;
    private uint _scanSequence;

    public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;

    public OleiLidarDriver(string deviceId, string deviceName, IConfigurationSection connection)
        : base(deviceId, deviceName, DeviceType.Lidar)
    {
        _connect.device_IP = connection.GetValue<string>("DeviceIp") ?? "192.168.254.13";
        _connect.device_port = connection.GetValue<int?>("DevicePort")
            ?? connection.GetValue<int?>("UdpPort")
            ?? 2368;
        _connect.local_ip = connection.GetValue<string>("LocalIp") ?? string.Empty;
        _connect.multiaddr_ip = connection.GetValue<string>("Multiaddr") ?? string.Empty;

        var minAngleDeg = connection.GetValue<double?>("MinAngle") ?? -135.0;
        var maxAngleDeg = connection.GetValue<double?>("MaxAngle") ?? 135.0;
        _startAngleRad = minAngleDeg * Math.PI / 180.0;
        _endAngleRad = maxAngleDeg * Math.PI / 180.0;

        _angleMinHundredths = (int)(minAngleDeg * 100 + 18000);
        _angleMaxHundredths = (int)(maxAngleDeg * 100 + 18000);

        _minRangeM = connection.GetValue<double?>("MinRangeM") ?? 0.2;
        _maxRangeM = connection.GetValue<double?>("MaxRangeM") ?? 50.0;
        _scanFrequencyHz = connection.GetValue<double?>("ScanFrequencyHz");
        _supportsIntensity = connection.GetValue<bool?>("SupportsIntensity") ?? true;
        _accuracyM = connection.GetValue<double?>("AccuracyM") ?? 0.02;
        _frameId = connection.GetValue<string>("FrameId") ?? "olei_lidar_frame";
        _poly = connection.GetValue<int?>("Poly") ?? 1;

        var stepDeg = connection.GetValue<double?>("StepDeg") ?? 0.225;
        _angularResolutionRad = stepDeg * Math.PI / 180.0;

        var decoderConfig = new DecoderConfig
        {
            AngleMin = minAngleDeg,
            AngleMax = maxAngleDeg,
            RangeMin = _minRangeM,
            RangeMax = _maxRangeM,
            Poly = _poly,
            Inverted = connection.GetValue<bool?>("Inverted") ?? false,
            StepDeg = stepDeg,
            FrameId = _frameId
        };
        _decode.SetConfig(decoderConfig);

        if (connection.GetValue<bool?>("AutoReconnectEnabled") is bool autoReconnect)
            AutoReconnectEnabled = autoReconnect;
        else
            AutoReconnectEnabled = true;

        if (connection.GetValue<int?>("ReconnectDelayMs") is int reconnectDelay)
            ReconnectDelayMs = reconnectDelay;

        if (connection.GetValue<int?>("MaxReconnectAttempts") is int maxAttempts)
            MaxReconnectAttempts = maxAttempts;

        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 = "0"
        };

        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 = (_startAngleRad * 180.0 / Math.PI).ToString("F1")
        };

        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 = (_endAngleRad * 180.0 / Math.PI).ToString("F1")
        };

        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 = _maxRangeM.ToString("F1")
        };

        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 Task OnInitializeAsync(CancellationToken cancellationToken)
    {
        lock (_dataLock)
        {
            _portOpened = _connect.openPort();
            if (!_portOpened)
                throw new InvalidOperationException(
                    $"Failed to open Olei UDP port {_connect.device_port} on local {_connect.local_ip}");
        }
        return Task.CompletedTask;
    }

    protected override Task OnConnectAsync(CancellationToken cancellationToken)
    {
        StartUpdateLoop();
        return Task.CompletedTask;
    }

    protected override Task OnDisconnectAsync(CancellationToken cancellationToken)
    {
        StopUpdateLoop();
        return Task.CompletedTask;
    }

    protected override Task OnResetAsync(CancellationToken cancellationToken)
    {
        lock (_dataLock)
        {
            _lastLaserScan = null;
            _lastScanDataTimestamp = null;
            _scanSequence = 0;
        }
        UpdateProperties();
        return Task.CompletedTask;
    }

    protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
    {
        if (!_portOpened || !_connect.IsOpen)
            return false;

        // LiDAR may need a few seconds after motor start before UDP data arrives.
        for (var i = 0; i < 40; i++)
        {
            if (_lastScanDataTimestamp.HasValue)
                return true;

            if (_connect.TotalPacketsReceived > 0 || _connect.GetPacketCount() > 0)
                return true;

            await Task.Delay(250, cancellationToken);
        }

        Console.WriteLine(
            $"[WARN] Olei LiDAR connection check failed: packets={_connect.TotalPacketsReceived}, " +
            $"queued={_connect.GetPacketCount()}, wrongSrc={_connect.DroppedWrongSource}, " +
            $"wrongSize={_connect.DroppedWrongSize}, device={_connect.device_IP}:{_connect.device_port}");

        return false;
    }

    private void StartUpdateLoop()
    {
        lock (_dataLock)
        {
            if (_updateCts != null)
                return;

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

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

    private async Task UpdateLoopAsync(CancellationToken cancellationToken)
    {
        while (!cancellationToken.IsCancellationRequested)
        {
            try
            {
                var framePublished = false;
                while (_connect.TryDequeuePacket() is { } pkt)
                {
                    if (_decode.PacketCb(pkt))
                        framePublished |= TryPublishScan(pkt.stamp);
                }

                if (!framePublished)
                    await Task.Delay(1, cancellationToken);
            }
            catch (OperationCanceledException)
            {
                break;
            }
            catch (Exception ex)
            {
                LastError = ex;
                OnErrorOccurred(ex, "Olei LiDAR update loop error");
                await Task.Delay(500, cancellationToken);
            }
        }
    }

    private bool TryPublishScan(DateTime packetReceiveStamp)
    {
        var angles = _decode.scanAngleInVec;
        var rangesMm = _decode.scanRangeInVec;
        var intensitiesRaw = _decode.scanIntensityInVec;

        if (angles.Count == 0 ||
            angles.Count != rangesMm.Count ||
            angles.Count != intensitiesRaw.Count)
            return false;

        var rangeList = new List<double>();
        var intensityList = new List<double>();
        var angleRadList = new List<double>();

        for (var i = 0; i < angles.Count; i++)
        {
            if (i % _poly != 0)
                continue;

            var angleHundredths = angles[i];
            if (angleHundredths < _angleMinHundredths || angleHundredths > _angleMaxHundredths)
                continue;

            var angleDeg = (angleHundredths - 18000) / 100.0;
            var angleRad = angleDeg * Math.PI / 180.0;
            angleRadList.Add(angleRad);

            var distanceM = rangesMm[i] * OleiConstants.DistanceResolution;
            var isValid = distanceM >= _minRangeM && distanceM <= _maxRangeM;
            rangeList.Add(isValid ? distanceM : double.NaN);
            intensityList.Add(intensitiesRaw[i]);
        }

        if (rangeList.Count == 0)
            return false;

        var frequency = _decode.Frequency > 0.001f
            ? _decode.Frequency
            : (float)(_scanFrequencyHz ?? 10.0);
        var scanTime = 1.0 / frequency;
        var numberOfPoints = rangeList.Count;

        var angleMin = angleRadList[0];
        var angleMax = angleRadList[^1];
        var angleIncrement = numberOfPoints > 1
            ? (angleMax - angleMin) / (numberOfPoints - 1)
            : _decode.StepDeg * Math.PI / 180.0;

        var receiveUtc = packetReceiveStamp.Kind == DateTimeKind.Utc
            ? packetReceiveStamp
            : packetReceiveStamp.ToUniversalTime();
        var lidarEndUtc = _decode.LastCompletedScanLidarEndUtc ?? receiveUtc;
        var scanStamp = lidarEndUtc - TimeSpan.FromSeconds(scanTime);

        var laserScan = new LaserScan
        {
            Header = new RobotNet10.Shared.Header
            {
                Seq = _scanSequence++,
                Stamp = scanStamp,
                FrameId = _frameId
            },
            AngleMin = angleMin,
            AngleMax = angleMax,
            AngleIncrement = angleIncrement,
            TimeIncrement = scanTime / numberOfPoints,
            ScanTime = scanTime,
            RangeMin = (float)_minRangeM,
            RangeMax = (float)_maxRangeM,
            Ranges = rangeList.ToArray(),
            Intensities = intensityList.ToArray()
        };

        lock (_dataLock)
        {
            _lastLaserScan = laserScan;
            _lastScanDataTimestamp = scanStamp;
        }

        ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(scanStamp, laserScan));
        UpdateProperties();
        return true;
    }

    private void UpdateProperties()
    {
        lock (_dataLock)
        {
            var pointCount = _lastLaserScan?.Ranges.Length ?? 0;
            SetProperty("NumberOfPoints", pointCount.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

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

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

    public double MinAngleRad => _startAngleRad;
    public double MaxAngleRad => _endAngleRad;
    public double MinRangeM => _minRangeM;
    public double MaxRangeM => _maxRangeM;
    public double? AngularResolutionRad => _angularResolutionRad;
    public double? ScanFrequencyHz => _decode.Frequency > 0.001f ? _decode.Frequency : _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();
            _connect.Dispose();
        }
        base.Dispose(disposing);
    }
}