using System.Diagnostics;
using Olei.LidarSensor;
using RobotNet10.RobotApp.Client.Shared.Devices;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared;
using RobotNet10.Shared.Sensor;

namespace RobotNet10.RobotApp.Drivers.Olei;

/// <summary>
/// Configuration for Olei 2D LiDAR Driver
/// </summary>
public class Olei2dLidarDriverConfig
{
    /// <summary>
    /// UDP port to listen for LiDAR data
    /// Default: 2368 (typical LiDAR port)
    /// </summary>
    public int UdpPort { get; set; } = 2368;

    /// <summary>
    /// Frame ID for the LiDAR sensor
    /// Used in ROS-style message headers
    /// </summary>
    public string FrameId { get; set; } = "laser";

    /// <summary>
    /// Whether the LiDAR is mounted inverted (upside down).
    /// When true, scan angles are mirrored (180° - angle) to correct orientation.
    /// </summary>
    public bool Inverted { get; set; } = false;
}

/// <summary>
/// Driver for Olei 2D LiDAR Sensor (LR-1F / LR-1BS)
/// Implements DeviceBase and ILidar interface
/// Protocol: UDP/IP v2.1
/// </summary>
[Device(DeviceType.Lidar, "Olei", "Olei2dLidarDriver", "1.0.0",
    Description = "2D LiDAR Sensor Communication Data Protocol v2.1")]
public class Olei2dLidarDriver : DeviceBase, ILidar
{
    private readonly Olei2dLidarDriverConfig _config = new();
    private OleiLidarServer? _lidarServer;

    // Cached measurements
    private LaserScan? _currentMeasurementData;
    private DateTime? _lastScanDataTimestamp;

    // Frequency update tracking (for UI properties)
    private readonly Stopwatch _frequencyUpdateStopwatch = Stopwatch.StartNew();

    // Scan frequency calculation (actual LaserScan generation rate)
    private readonly Stopwatch _scanFrequencyStopwatch = Stopwatch.StartNew();
    private long _scansGeneratedInCurrentSecond = 0;
    private readonly Lock _scanFrequencyLock = new();

    // Calculated LiDAR specifications from actual data
    private const double _minAngleRad = 0.0;
    private const double _maxAngleRad = 2.0 * Math.PI;
    private double _angularResolutionRad = 0.0;  // Calculated from actual packet data
    private const double DEFAULT_ACCURACY_M = 0.02;   // 2 cm accuracy (fixed by hardware)

    // Hardware range specifications (no filtering applied)
    private const double HARDWARE_MIN_RANGE_M = 0.0;
    private const double HARDWARE_MAX_RANGE_M = 30.0;  // 30m max range for Olei LiDAR

    // Packet accumulation for full scan (0° ~ 360°)
    private readonly List<LidarDataBlock> _accumulatedBlocks = [];
    private double _lastPacketStartAngle = -1.0;
    private readonly Lock _accumulationLock = new();
    private uint _scanSequenceNumber = 0;
    private DateTime _currentScanStartTime = DateTime.UtcNow;

    /// <summary>
    /// Constructor with configuration
    /// </summary>
    public Olei2dLidarDriver(
        string deviceId,
        string deviceName, IConfigurationSection configuration)
        : base(deviceId, deviceName, DeviceType.Lidar)
    {
        configuration.Bind(_config);
        Description = "Olei 2D LiDAR Sensor (LR-1F/LR-1BS) - UDP Protocol v2.1";
    }

    #region ILidar Implementation

    /// <summary>
    /// Current measurement data (scan points)
    /// </summary>
    public LaserScan? CurrentMeasurementData => _currentMeasurementData;

    /// <summary>
    /// Timestamp of the most recent scan data
    /// </summary>
    public DateTime? LastScanDataTimestamp => _lastScanDataTimestamp;

    /// <summary>
    /// Minimum scan angle (radians)
    /// Calculated from actual LiDAR data
    /// </summary>
    public double MinAngleRad => _minAngleRad;

    /// <summary>
    /// Maximum scan angle (radians)
    /// Calculated from actual LiDAR data
    /// </summary>
    public double MaxAngleRad => _maxAngleRad;

    /// <summary>
    /// Minimum measurement range (meters)
    /// Hardware specification (no filtering)
    /// </summary>
    public double MinRangeM => HARDWARE_MIN_RANGE_M;

    /// <summary>
    /// Maximum measurement range (meters)
    /// Hardware specification (no filtering)
    /// </summary>
    public double MaxRangeM => HARDWARE_MAX_RANGE_M;

    /// <summary>
    /// Angular resolution (radians)
    /// Calculated from actual LiDAR data
    /// </summary>
    public double? AngularResolutionRad => _angularResolutionRad;

    /// <summary>
    /// Scan frequency (Hz)
    /// Typically 10-20 Hz for Olei LiDAR
    /// </summary>
    public double? ScanFrequencyHz { get; private set; }

    /// <summary>
    /// Field of View (radians)
    /// </summary>
    public double FieldOfViewRad => MaxAngleRad - MinAngleRad;

    /// <summary>
    /// Supports intensity measurements
    /// </summary>
    public bool SupportsIntensity => true;

    /// <summary>
    /// Measurement accuracy (meters)
    /// </summary>
    public double? AccuracyM => DEFAULT_ACCURACY_M;

    /// <summary>
    /// Event raised when new scan data is received
    /// </summary>
    public event EventHandler<LidarScanDataEventArgs>? ScanDataReceived;

    #endregion

    #region DeviceBase Implementation

    protected override async Task OnInitializeAsync(CancellationToken cancellationToken)
    {
        await Task.Run(() =>
        {
            // Initialize LiDAR server
            _lidarServer = new OleiLidarServer(_config.UdpPort);

            // Subscribe to events
            _lidarServer.DataReceived += OnLidarDataReceived;
            _lidarServer.ErrorOccurred += OnLidarErrorOccurred;

            SetProperty("UdpPort", _config.UdpPort.ToString());
            SetProperty("FrameId", _config.FrameId);
            SetProperty("MinRange", $"{HARDWARE_MIN_RANGE_M:F2} m");
            SetProperty("MaxRange", $"{HARDWARE_MAX_RANGE_M:F2} m");
            SetProperty("FOV", "N/A");  // Will be calculated from actual data
            SetProperty("AngularResolution", "N/A");  // Will be calculated from actual data

        }, cancellationToken);
    }

    protected override async Task OnConnectAsync(CancellationToken cancellationToken)
    {
        if (_lidarServer == null)
            throw new InvalidOperationException("LiDAR server not initialized. Call InitializeAsync first.");

        // Start LiDAR server
        _lidarServer.Start();

        SetProperty("ServerStatus", "Running");
        SetProperty("Statistics", _lidarServer.GetStatistics());
    }

    protected override async Task OnDisconnectAsync(CancellationToken cancellationToken)
    {
         _lidarServer?.Stop();

        SetProperty("ServerStatus", "Stopped");
        SetProperty("Statistics", _lidarServer?.GetStatistics() ?? "N/A");
    }

    protected override async Task OnResetAsync(CancellationToken cancellationToken)
    {
        // Reset statistics
        _lidarServer?.ResetStatistics();

        // Reset scan frequency tracking
        lock (_scanFrequencyLock)
        {
            Interlocked.Exchange(ref _scansGeneratedInCurrentSecond, 0);
            _scanFrequencyStopwatch.Restart();
            ScanFrequencyHz = null;
        }

        SetProperty("Statistics", _lidarServer?.GetStatistics() ?? "N/A");
        SetProperty("ScanFrequency", "N/A");
    }

    protected override async Task<bool> OnCheckConnectionAsync(CancellationToken cancellationToken)
    {
        await Task.Delay(500, cancellationToken);
        if (_lidarServer == null || !_lidarServer.IsRunning)
            return false;

        lock (_accumulationLock)
        {
            return _lastScanDataTimestamp.HasValue;
        }
    }

    protected override List<PropertyDescription> CreatePropertyDescriptions()
    {
        return
        [
            new PropertyDescription("UdpPort", "UDP Port", "UDP port for receiving LiDAR data"),
            new PropertyDescription("FrameId", "Frame ID", "ROS-style frame identifier"),
            new PropertyDescription("MinRange", "Min Range", "Minimum measurement range"),
            new PropertyDescription("MaxRange", "Max Range", "Maximum measurement range"),
            new PropertyDescription("FOV", "Field of View", "Total scanning angle"),
            new PropertyDescription("AngularResolution", "Angular Resolution", "Angle between scan points"),
            new PropertyDescription("ServerStatus", "Server Status", "UDP server status"),
            new PropertyDescription("Statistics", "Statistics", "Server statistics"),
            new PropertyDescription("LastScanTime", "Last Scan Time", "Timestamp of last scan"),
            new PropertyDescription("ScanFrequency", "Scan Frequency", "Actual scan rate (Hz)"),
        ];
    }

    protected override void Dispose(bool disposing)
    {
        if (disposing && _lidarServer != null)
        {
            _lidarServer.DataReceived -= OnLidarDataReceived;
            _lidarServer.ErrorOccurred -= OnLidarErrorOccurred;
            _lidarServer.Dispose();
            _lidarServer = null;
        }

        base.Dispose(disposing);
    }

    #endregion

    #region Event Handlers

    /// <summary>
    /// Handle data received from LiDAR server
    /// Accumulates packets until full 360° scan is complete
    /// </summary>
    private void OnLidarDataReceived(object? sender, LidarDataPacket e)
    {
        try
        {
            var packet = e;

            // Validate packet
            if (!packet.Header.IsValidFrame)
                return;

            // Get first valid block's angle to detect scan wrap
            double packetStartAngle = -1.0;
            for (int i = 0; i < LidarDataPacket.DATA_BLOCK_COUNT; i++)
            {
                if (packet.DataBlocks[i].IsValid)
                {
                    packetStartAngle = packet.DataBlocks[i].GetAngleDegrees();
                    break;
                }
            }

            if (packetStartAngle < 0)
                return; // No valid blocks in packet

            LaserScan? completedScan = null;
            bool updateProperties = false;

            lock (_accumulationLock)
            {
                // Detect scan completion: angle wrapped back to near 0° after being > 300°
                if (_lastPacketStartAngle > 300.0 && packetStartAngle < 50.0 && _accumulatedBlocks.Count > 0)
                {
                    var scan = CreateLaserScanFromAccumulated(packet.Header, _currentScanStartTime);
                    completedScan = scan;

                    _currentMeasurementData = scan;
                    _lastScanDataTimestamp = scan.Header.Stamp;

                    Interlocked.Increment(ref _scansGeneratedInCurrentSecond);
                    UpdateScanFrequency();

                    if (_frequencyUpdateStopwatch.ElapsedMilliseconds > 1000)
                    {
                        updateProperties = true;
                        _frequencyUpdateStopwatch.Restart();
                    }

                    _accumulatedBlocks.Clear();
                    _currentScanStartTime = DateTime.UtcNow;
                }
                else if (_accumulatedBlocks.Count == 0)
                {
                    _currentScanStartTime = DateTime.UtcNow;
                }

                // Add current packet's blocks to accumulation buffer
                for (int i = 0; i < LidarDataPacket.DATA_BLOCK_COUNT; i++)
                {
                    var block = packet.DataBlocks[i];
                    if (block.IsValid)
                    {
                        double angleDeg = block.GetAngleDegrees();
                        if (angleDeg >= 360.0)
                            angleDeg %= 360.0;
                        _accumulatedBlocks.Add(block);
                    }
                }

                _lastPacketStartAngle = packetStartAngle;
            }

            // Fire event and update properties outside the lock to avoid blocking UDP reception
            if (completedScan is { } publishedScan)
            {
                if (updateProperties)
                {
                    SetProperty("LastScanTime", publishedScan.Header.Stamp.ToString("HH:mm:ss.fff"));
                    SetProperty("ScanFrequency", ScanFrequencyHz.HasValue ? $"{ScanFrequencyHz.Value:F2} Hz" : "N/A");
                    SetProperty("FOV", $"{FieldOfViewRad * 180 / Math.PI:F1}°");
                    SetProperty("AngularResolution", $"{_angularResolutionRad * 180 / Math.PI:F3}°");
                }

                ScanDataReceived?.Invoke(this, new LidarScanDataEventArgs(
                    publishedScan.Header.Stamp,
                    publishedScan
                ));
            }
        }
        catch (Exception ex)
        {
            OnErrorOccurred(new Exception($"Error processing LiDAR data: {ex.Message}", ex));
        }
    }

    /// <summary>
    /// Handle errors from LiDAR server
    /// </summary>
    private void OnLidarErrorOccurred(object? sender, LidarErrorEventArgs e)
    {
        OnErrorOccurred(e.Exception ?? new Exception(e.Message));
    }

    #endregion

    #region Helper Methods

    /// <summary>
    /// Update scan frequency based on LaserScan generation rate
    /// Calculates how many scans are generated per second
    /// </summary>
    private void UpdateScanFrequency()
    {
        // Check if 1 second has elapsed
        if (_scanFrequencyStopwatch.ElapsedMilliseconds >= 1000)
        {
            lock (_scanFrequencyLock)
            {
                // Double-check inside lock to prevent race condition
                if (_scanFrequencyStopwatch.ElapsedMilliseconds >= 1000)
                {
                    // Calculate frequency (scans per second)
                    long scanCount = Interlocked.Read(ref _scansGeneratedInCurrentSecond);
                    double elapsedSeconds = _scanFrequencyStopwatch.ElapsedMilliseconds / 1000.0;
                    ScanFrequencyHz = scanCount / elapsedSeconds;

                    // Reset for next measurement period
                    Interlocked.Exchange(ref _scansGeneratedInCurrentSecond, 0);
                    _scanFrequencyStopwatch.Restart();
                }
            }
        }
    }

    /// <summary>
    /// Create LaserScan from accumulated blocks (full 360° scan)
    /// </summary>
    private LaserScan CreateLaserScanFromAccumulated(LidarHeader lastHeader, DateTime scanStartTime)
    {
        var header = new Header(
            seq: _scanSequenceNumber++,
            stamp: scanStartTime,
            frameId: _config.FrameId
        );

        // Get distance scale from last header
        byte distanceScale = lastHeader.DistanceScale;
        double distanceScaleToMeters = distanceScale / 1000.0;

        // Calculate actual angle and range from accumulated data
        double minAngleDeg = double.MaxValue;
        double maxAngleDeg = double.MinValue;
        double minDistanceM = double.MaxValue;
        double maxDistanceM = double.MinValue;

        // Create sorted list of angles to calculate angular resolution
        var sortedAngles = new List<double>(_accumulatedBlocks.Count);

        // First pass: Find actual min/max values from data and collect angles
        foreach (var block in _accumulatedBlocks)
        {
            double angleDeg = block.GetAngleDegrees();

            // Normalize angle to 0-360° range
            if (angleDeg >= 360.0)
                angleDeg %= 360.0;

            minAngleDeg = Math.Min(minAngleDeg, angleDeg);
            maxAngleDeg = Math.Max(maxAngleDeg, angleDeg);
            sortedAngles.Add(angleDeg);

            double distanceM = block.DistanceRaw * distanceScaleToMeters;
            if (distanceM > 0)  // Only consider valid distances
            {
                minDistanceM = Math.Min(minDistanceM, distanceM);
                maxDistanceM = Math.Max(maxDistanceM, distanceM);
            }
        }

        // Fallback to defaults if no valid data
        if (minAngleDeg == double.MaxValue || maxAngleDeg == double.MinValue)
        {
            minAngleDeg = 0.0;
            maxAngleDeg = 360.0;
        }

        if (minDistanceM == double.MaxValue || maxDistanceM == double.MinValue)
        {
            minDistanceM = HARDWARE_MIN_RANGE_M;
            maxDistanceM = HARDWARE_MAX_RANGE_M;
        }

        // Calculate angular resolution from actual data
        double angularResolutionDeg = 0.225;  // Default fallback
        if (sortedAngles.Count > 1)
        {
            sortedAngles.Sort();

            // Find minimum difference between consecutive angles
            double minAngleDiff = double.MaxValue;
            for (int i = 1; i < sortedAngles.Count; i++)
            {
                double diff = sortedAngles[i] - sortedAngles[i - 1];
                if (diff > 0.01)  // Ignore very small differences (noise/duplicates)
                {
                    minAngleDiff = Math.Min(minAngleDiff, diff);
                }
            }

            if (minAngleDiff < double.MaxValue)
            {
                angularResolutionDeg = minAngleDiff;
            }
        }

        // Update cached angular resolution for property reporting
        _angularResolutionRad = angularResolutionDeg * Math.PI / 180.0;

        // Convert angles to radians
        double angleMinRad = minAngleDeg * Math.PI / 180.0;
        double angleMaxRad = maxAngleDeg * Math.PI / 180.0;

        // Calculate expected number of points based on calculated angular resolution
        double angleSpanDeg = maxAngleDeg - minAngleDeg;
        int expectedPoints = (int)Math.Ceiling(angleSpanDeg / angularResolutionDeg) + 1;

        // Initialize arrays with expected size
        double[] ranges = new double[expectedPoints];
        double[] intensities = new double[expectedPoints];

        // Initialize all to -1 (no data, JSON-safe)
        Array.Fill(ranges, -1.0);
        Array.Fill(intensities, 0.0);

        // Second pass: Map accumulated blocks to array indices based on angle
        int validCount = 0;
        foreach (var block in _accumulatedBlocks)
        {
            double angleDeg = _config.Inverted
                ? (block.GetAngleDegrees() + 180.0) % 360.0
                : block.GetAngleDegrees();

            // Normalize angle to 0-360° range
            if (angleDeg < 0)
                angleDeg += 360.0;
            else if (angleDeg >= 360.0)
                angleDeg %= 360.0;

            // Calculate array index from relative angle position using calculated angular resolution
            double relativeAngle = angleDeg - minAngleDeg;
            int index = (int)Math.Round(relativeAngle / angularResolutionDeg);

            // Clamp index to valid range
            if (index >= 0 && index < expectedPoints)
            {
                double distanceM = block.DistanceRaw * distanceScaleToMeters;
                if (distanceM <= 0 || distanceM >= HARDWARE_MAX_RANGE_M)
                {
                    ranges[index] = -1.0;  // No detection / max-range return
                    intensities[index] = 0.0;
                    continue;
                }
                // Store all distance values (no range filtering)
                ranges[index] = distanceM;
                intensities[index] = block.SignalStrength;
                validCount++;
            }
        }

        // Calculate angle increment from actual data
        double angleIncrementRad = expectedPoints > 1
            ? (angleMaxRad - angleMinRad) / (expectedPoints - 1)
            : angularResolutionDeg * Math.PI / 180.0;  // Use calculated resolution

        // Calculate scan timing (estimate based on rotation rate if available)
        double scanTime = ScanFrequencyHz.HasValue && ScanFrequencyHz.Value > 0
            ? 1.0 / ScanFrequencyHz.Value
            : 0.1; // Default 10 Hz
        double timeIncrement = scanTime / expectedPoints;

        return new LaserScan
        {
            Header = header,
            AngleMin = angleMinRad,       // From actual data
            AngleMax = angleMaxRad,       // From actual data
            AngleIncrement = angleIncrementRad,  // Calculated from data
            TimeIncrement = timeIncrement,
            ScanTime = scanTime,
            RangeMin = HARDWARE_MIN_RANGE_M,
            RangeMax = HARDWARE_MAX_RANGE_M,
            Ranges = ranges,
            Intensities = intensities
        };
    }

    #endregion
}