using Microsoft.Extensions.Configuration;
using Microsoft.Extensions.Hosting;
using Microsoft.Extensions.Logging;
using RobotNet10.RobotApp.Devices;
using RobotNet10.Shared;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Sensor;
using System;
using System.Threading;
using System.Threading.Tasks;

namespace RobotNet10.RobotApp.MarkerDetection;

/// <summary>
/// Integration service to dispatch laser scan data to marker detection engine
/// Provides rectangle or segment marker detection with pose estimation
/// </summary>
public class MarkerDetectionIntegrationService : IHostedService, IDisposable
{
    private readonly MarkerDetectionIntegrationConfiguration _config;
    private readonly IDeviceProvider _deviceProvider;
    private readonly ILogger<MarkerDetectionIntegrationService> _logger;
    private readonly object _lock = new();
    private readonly SemaphoreSlim _dispatchSemaphore = new SemaphoreSlim(1, 1);  // Only 1 dispatch at a time

    private MarkerDetectionClient? _markerDetectionClient;
    
    // Timer for periodic marker pose updates and logging
    private Timer? _updateTimer;
    
    // Cached device references
    private ILidar? _lidarDevice;
    
    // Statistics (accessed from multiple threads)
    private long _laserScanCount = 0;
    private long _markerDetectionCount = 0;
    private DateTime _lastStatsLog = DateTime.UtcNow;

    private bool _disposed = false;
    private bool _isInitialized = false;

    public MarkerDetectionIntegrationService(
        IConfiguration configuration,
        IDeviceProvider deviceProvider,
        ILogger<MarkerDetectionIntegrationService> logger)
    {
        _deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
        _logger = logger ?? throw new ArgumentNullException(nameof(logger));

        // Load configuration
        var configSection = configuration.GetSection("MarkerDetection:Integration");
        if (!configSection.Exists())
        {
            _logger.LogWarning("Configuration section 'MarkerDetection:Integration' not found. Using defaults.");
            _config = new MarkerDetectionIntegrationConfiguration();
        }
        else
        {
            _config = new MarkerDetectionIntegrationConfiguration();
            configSection.Bind(_config);
        }

        ValidateConfiguration();
    }

    private void ValidateConfiguration()
    {
        if (_config.Enabled)
        {
            if (_config.LaserScanDispatchIntervalMs <= 0 || _config.LaserScanDispatchIntervalMs > 1000)
            {
                _logger.LogWarning("LaserScanDispatchIntervalMs {Interval} is out of range [1-1000]. Using default 50 ms.", _config.LaserScanDispatchIntervalMs);
                _config.LaserScanDispatchIntervalMs = 50;
            }

            if (_config.MarkerType == MarkerDetectionType.Rectangle && _config.RectangleMarkerOptions == null)
            {
                _logger.LogWarning("Rectangle marker type selected but options not provided. Using defaults.");
                _config.RectangleMarkerOptions = new MarkerDetectionRectangleOptions();
            }

            if (_config.MarkerType == MarkerDetectionType.Segment && _config.SegmentMarkerOptions == null)
            {
                _logger.LogWarning("Segment marker type selected but options not provided. Using defaults.");
                _config.SegmentMarkerOptions = new MarkerDetectionSegmentOptions();
            }
        }
    }

    public async Task StartAsync(CancellationToken cancellationToken)
    {
        if (!_config.Enabled)
        {
            _logger.LogInformation("MarkerDetectionIntegrationService is disabled in configuration");
            return;
        }

        _logger.LogInformation("Starting MarkerDetectionIntegrationService...");

        try
        {
            // Wait for devices to be connected
            _logger.LogInformation("Waiting for devices to be connected...");
            var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);

            if (!connected)
            {
                _logger.LogWarning("Timeout waiting for devices. MarkerDetectionIntegrationService will retry initialization.");
                _ = Task.Run(async () => await RetryInitializationAsync(cancellationToken), cancellationToken);
                return;
            }

            await InitializeAsync(cancellationToken);
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error starting MarkerDetectionIntegrationService: {Message}", ex.Message);
        }
    }

    private async Task RetryInitializationAsync(CancellationToken cancellationToken)
    {
        const int maxRetries = 60; // 5 minutes with 5 second intervals
        int retryCount = 0;

        while (retryCount < maxRetries && !cancellationToken.IsCancellationRequested && !_isInitialized)
        {
            await Task.Delay(TimeSpan.FromSeconds(5), cancellationToken);

            var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromSeconds(1), cancellationToken);
            if (connected)
            {
                _logger.LogInformation("Devices are now connected. Initializing MarkerDetectionIntegrationService...");
                await InitializeAsync(cancellationToken);
                return;
            }

            retryCount++;
            if (retryCount % 12 == 0) // Log every minute
            {
                _logger.LogInformation("Still waiting for devices... (attempt {Attempt}/{MaxAttempts})",
                    retryCount, maxRetries);
            }
        }

        if (retryCount >= maxRetries)
        {
            _logger.LogWarning("Timeout waiting for devices. MarkerDetectionIntegrationService will not be initialized.");
        }
    }

    private async Task InitializeAsync(CancellationToken cancellationToken)
    {
        lock (_lock)
        {
            if (_isInitialized)
                return;

            try
            {
                // Get LIDAR device reference
                if (!string.IsNullOrEmpty(_config.LidarDeviceId))
                {
                    var device = _deviceProvider.GetDevice(_config.LidarDeviceId);
                    if (device != null)
                    {
                        _logger.LogInformation(
                            "[LIDAR-INIT] Device '{DeviceId}' found. Type: {DeviceType}, IsConnected: {IsConnected}",
                            _config.LidarDeviceId, device.GetType().Name, device.IsConnected);
                    }
                    
                    if (device is ILidar lidar && device.IsConnected)
                    {
                        _lidarDevice = lidar;
                        _logger.LogInformation(
                            "[LIDAR-INIT] SUCCESS: LIDAR device '{DeviceId}' is initialized and connected. Ready to receive laser scan data.",
                            _config.LidarDeviceId);
                    }
                    else
                    {
                        _logger.LogWarning("LIDAR device '{DeviceId}' not found or not connected. Marker detection will not work.",
                            _config.LidarDeviceId);
                        return;
                    }
                }
                else
                {
                    _logger.LogWarning("No LIDAR device ID configured. Marker detection will not work.");
                    return;
                }

                // Create and initialize marker detection client
                _markerDetectionClient = new MarkerDetectionClient(_logger);

                // Initialize with appropriate marker type
                if (_config.MarkerType == MarkerDetectionType.Rectangle && _config.RectangleMarkerOptions != null)
                {
                    _markerDetectionClient.InitializeRectangle(_config.RectangleMarkerOptions);
                }
                else if (_config.MarkerType == MarkerDetectionType.Segment && _config.SegmentMarkerOptions != null)
                {
                    _markerDetectionClient.InitializeSegment(_config.SegmentMarkerOptions);
                }
                else
                {
                    throw new InvalidOperationException("Invalid marker type configuration or missing marker options.");
                }

                _logger.LogInformation("MarkerDetectionClient initialized successfully");

                // Start laser scan dispatch loop as background task (non-blocking)
                if (_lidarDevice != null)
                {
                    _ = Task.Run(async () => await LaserScanDispatchLoopAsync(cancellationToken), cancellationToken);
                    _logger.LogInformation("Laser scan dispatch loop started at {Rate} Hz", 1000.0 / _config.LaserScanDispatchIntervalMs);
                }

                // Start periodic update timer for pose logging and status updates
                _updateTimer = new Timer(UpdatePoseAndDiagnostics, null,
                    TimeSpan.FromSeconds(2), TimeSpan.FromSeconds(2));
                _logger.LogInformation("MarkerDetection update timer started (every 2 seconds)");

                _isInitialized = true;
                _logger.LogInformation("MarkerDetectionIntegrationService initialized successfully");
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Failed to initialize MarkerDetectionIntegrationService: {Message}", ex.Message);
                throw;
            }
        }
    }

    public async Task StopAsync(CancellationToken cancellationToken)
    {
        _logger.LogInformation("Stopping MarkerDetectionIntegrationService...");

        try
        {
            lock (_lock)
            {
                _isInitialized = false;
                
                // Dispose timer
                _updateTimer?.Dispose();
                _updateTimer = null;
            }

            // Wait a bit for dispatch loops to notice _isInitialized = false and stop
            _logger.LogInformation("Waiting for dispatch loops to stop...");
            await Task.Delay(500, cancellationToken);

            // Clean up marker detection client
            if (_markerDetectionClient != null)
            {
                _markerDetectionClient.Dispose();
                _markerDetectionClient = null;
                _logger.LogInformation("MarkerDetectionClient disposed successfully");
            }

            LogFinalStatistics();
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error stopping MarkerDetectionIntegrationService: {Message}", ex.Message);
        }

        await Task.CompletedTask;
    }

    /// <summary>
    /// Laser scan dispatch loop - runs in background task
    /// </summary>
    private async Task LaserScanDispatchLoopAsync(CancellationToken cancellationToken)
    {
        int noDataCounter = 0;
        
        while (_config.Enabled && !cancellationToken.IsCancellationRequested)
        {
            if (!_isInitialized || _markerDetectionClient == null || _lidarDevice == null || !_markerDetectionClient.IsInitialized)
            {
                await Task.Delay(100, cancellationToken);
                continue;
            }

            try
            {
                var scan = _lidarDevice.CurrentMeasurementData;
                
                // Log periodically to monitor intensities status (every ~5 seconds at 20Hz)
                if (scan.HasValue && _laserScanCount % 100 == 0)
                {
                    var rawScan = scan.Value;
                    if (rawScan.Intensities == null || rawScan.Intensities.Length == 0)
                    {
                        _logger.LogWarning(
                            "[MARKER-DETECTION] LIDAR NOT providing intensities (Ranges: {RangeCount}). " +
                            "Using default values. Check LIDAR RSSI configuration.",
                            rawScan.Ranges?.Length ?? 0);
                    }
                    // else
                    // {
                    //     _logger.LogInformation(
                    //         "[MARKER-DETECTION] ✓ LIDAR providing REAL intensities: {Count} values, " +
                    //         "Sample: [{I0:F3}, {I1:F3}, {I2:F3}]",
                    //         rawScan.Intensities.Length,
                    //         rawScan.Intensities[0],
                    //         rawScan.Intensities.Length > 1 ? rawScan.Intensities[1] : 0,
                    //         rawScan.Intensities.Length > 2 ? rawScan.Intensities[2] : 0);
                    // }
                }
                if (scan == null)
                {
                    noDataCounter++;
                    if (noDataCounter % 100 == 0)  // Log every 100 iterations (5 seconds at 20Hz)
                    {
                        // _logger.LogWarning(
                        //     "[LIDAR-DATA-DEBUG] CurrentMeasurementData is NULL. " +
                        //     "LIDAR device may not be connected or initialized. " +
                        //     "Configured device: '{LidarDeviceId}'. Missing data for {MissingIterations} iterations.",
                        //     _config.LidarDeviceId, noDataCounter);
                    }
                    continue;
                }
                
                var originalScan = scan.Value; // Assuming CurrentMeasurementData is nullable
                //in ra dữ liệu intensities và ranges để debug
                // _logger.LogInformation("LIDAR Data Debug - Ranges Count: {RangesCount}, Intensities Count: {IntensitiesCount}", 
                //     originalScan.Ranges?.Length ?? 0, originalScan.Intensities?.Length ?? 0);

                // Validate scan data before processing
                // LIDAR must provide at least range data (Intensities may be empty)
                if (originalScan.Ranges == null || originalScan.Ranges.Length == 0)
                {
                    noDataCounter++;
                    // if (noDataCounter % 100 == 0)
                    // {
                    //     _logger.LogWarning(
                    //         "[LIDAR-DATA-DEBUG] No RANGE data in laser scan. " +
                    //         "Ranges: {RangeLength}, Intensities: {IntensityLength}. " +
                    //         "LIDAR device '{LidarDeviceId}' is not providing valid range measurements. " +
                    //         "Missing data for {MissingIterations} iterations.",
                    //         originalScan.Ranges?.Length ?? 0,
                    //         originalScan.Intensities?.Length ?? 0,
                    //         _config.LidarDeviceId,
                    //         noDataCounter);
                    // }
                    continue;
                }
                
                // Ensure Intensities array is not null
                // If LIDAR doesn't provide intensities, populate with default values
                // if (originalScan.Intensities == null || originalScan.Intensities.Length == 0)
                // {
                //     originalScan.Intensities = new float[originalScan.Ranges.Length];
                //     for (int i = 0; i < originalScan.Intensities.Length; i++)
                //     {
                //         originalScan.Intensities[i] = 1.0f;  // Default fallback value
                //     }
                // }
                
                // Data is valid - reset counter
                noDataCounter = 0;
                
                // Log once per second when valid data is being received
                // if (noDataCounter == 0 && _laserScanCount % 20 == 0)  // ~1 second at 20Hz
                // {
                //     _logger.LogDebug(
                //         "[LIDAR-DATA-OK] Valid laser scan received: Ranges: {RangeLength}, Intensities: {IntensityLength}",
                //         originalScan.Ranges.Length,
                //         originalScan.Intensities.Length);
                // }

                // Create updated scan with current timestamp
                var updatedScan = new LaserScan
                {
                    Header = new Header
                    {
                        Seq = originalScan.Header.Seq,
                        Stamp = DateTime.UtcNow,
                        FrameId = "scan" 
                    },
                    AngleMin = (float)-2.356194496154785,
                    AngleMax = (float)2.356194496154785,
                    AngleIncrement =(float) 0.00581718236207962,
                    TimeIncrement = (float) 6.172839493956417e-05,
                    ScanTime = (float) 0.06666667014360428,
                    RangeMin = (float) 0.0,
                    RangeMax = (float) 100.0,
                    Ranges = originalScan.Ranges,
                    Intensities = originalScan.Intensities
                };
                // In ra tất cả giũ liệu trong mảng Intensities 
                // if (updatedScan.Intensities != null)
                // {
                //     _logger.LogDebug("Intensities array length: {Length}", updatedScan.Intensities.Length);
                //     for (int i = 0; i < Math.Min(811, updatedScan.Intensities.Length); i++)
                //     {
                //         _logger.LogInformation("Intensity[{Index}]: {Value}", i, originalScan.Intensities[i]);
                //         _logger.LogInformation("Range[{Index}]: {Value}", i, updatedScan.Ranges[i]);
                //     }
                // }
                // In ra tất cả giũ liệu trong mảng Ranges và Intensities trong cùng 1 dòng và Seq, Stamp, FrameId để debug
                // if (updatedScan.Intensities != null)
                // {
                //     for (int i = 0; i < Math.Min(811, updatedScan.Intensities.Length); i++)
                //     {
                //         _logger.LogInformation("Detect marker Seq: {Seq}, Stamp: {Stamp}, FrameId: {FrameId}, Range[{Index}]: {Range}, Intensity[{Index}]: {Intensity}",
                //             updatedScan.Header.Seq,
                //             updatedScan.Header.Stamp.ToString("HH:mm:ss.ffff"),
                //             updatedScan.Header.FrameId,
                //             i,
                //             updatedScan.Ranges[i],
                //             i,
                //             updatedScan.Intensities[i]);
                //     }
                // }
                // Dispatch laser scan to marker detection
                // Re-check that client is still valid (could have been disposed by another thread)
                if (_markerDetectionClient == null || !_markerDetectionClient.IsInitialized)
                {
                    continue;
                }

                await _dispatchSemaphore.WaitAsync(cancellationToken);
                try
                {
                    // Double-check again after acquiring semaphore
                    if (_markerDetectionClient != null && _markerDetectionClient.IsInitialized)
                    {
                        int rangeCount = updatedScan.Ranges?.Length ?? 0;
                        int intensityCount = updatedScan.Intensities?.Length ?? 0;
                        
                        // _logger.LogInformation(
                        //     "[MARKER-DETECTION-DISPATCH] Sending to native code - " +
                        //     "Ranges: {RangeCount}, Intensities: {IntensityStatus} (Count: {IntensityCount})",
                        //     rangeCount,
                        //     updatedScan.Intensities == null ? "NULL" : "OK",
                        //     intensityCount);
                        
                        // if (updatedScan.Intensities != null && intensityCount > 2)
                        // {
                        //     _logger.LogInformation(
                        //         "[MARKER-DETECTION-DISPATCH] First 3 intensities being sent: [{I0:F2}, {I1:F2}, {I2:F2}]",
                        //         updatedScan.Intensities[0],
                        //         updatedScan.Intensities[1],
                        //         updatedScan.Intensities[2]);
                        // }
                        
                        _markerDetectionClient.DispatchLaserScan(updatedScan, _config.LaserScanSensorId ?? "scan");
                        Interlocked.Increment(ref _laserScanCount);
                    }
                }
                finally
                {
                    _dispatchSemaphore.Release();
                }
            }
            catch (InvalidOperationException ex) when (ex.Message.Contains("not initialized"))
            {
                // MarkerDetectionClient was disposed during shutdown - this is normal, just exit gracefully
                _logger.LogDebug("Laser scan dispatch loop stopped: {Message}", ex.Message);
                break;
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error dispatching laser scan data: {Message}", ex.Message);
            }

            // Wait for next cycle
            await Task.Delay(_config.LaserScanDispatchIntervalMs, cancellationToken);
        }
    }

    /// <summary>
    /// Periodic timer callback to update marker pose and log diagnostics
    /// </summary>
    private void UpdatePoseAndDiagnostics(object? state)
    {
        if (!_isInitialized || _markerDetectionClient == null)
            return;

        try
        {
            // Get current marker pose if available
            if (_markerDetectionClient.IsInitialized)
            {
                var pose = _markerDetectionClient.GetMarkerPose();
                if (pose != null)
                {
                    Interlocked.Increment(ref _markerDetectionCount);
                    // _logger.LogDebug("Marker pose: x={X}, y={Y}, z={Z}, qx={Qx}, qy={Qy}, qz={Qz}, qw={Qw}, frame={Frame}",
                    //     pose.Pose.Position[0], pose.Pose.Position[1], pose.Pose.Position[2],
                    //     pose.Pose.Orientation[0], pose.Pose.Orientation[1], pose.Pose.Orientation[2], pose.Pose.Orientation[3],
                    //     pose.Header.FrameId);
                }
            }
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error updating marker pose and diagnostics: {Message}", ex.Message);
        }

        // Log statistics periodically
        var now = DateTime.UtcNow;
        if ((now - _lastStatsLog).TotalSeconds >= 10)
        {
            _lastStatsLog = now;
            LogStatistics();
        }
    }

    private void LogStatistics()
    {
        var laserCount = Interlocked.Read(ref _laserScanCount);
        var markerCount = Interlocked.Read(ref _markerDetectionCount);

        if (laserCount > 0)
        {
            var laserRate = laserCount / ((DateTime.UtcNow - _lastStatsLog).TotalSeconds + 0.001);
            // _logger.LogInformation(
            //     "[MarkerDetection Stats] LaserScans: {LaserCount} (avg {LaserRate:F2} Hz), Marker Poses: {MarkerCount}",
            //     laserCount, laserRate, markerCount);
        }
        else
        {
            // LIDAR data still empty - provide diagnostic info
            _logger.LogWarning(
                "[MarkerDetection Stats] No laser scan data received. " +
                "LIDAR Device ID configured: '{ConfigDeviceId}'. " +
                "Please verify LIDAR device is connected and providing data.",
                _config.LidarDeviceId);
        }
    }

    private void LogFinalStatistics()
    {
        var laserCount = Interlocked.Read(ref _laserScanCount);
        var markerCount = Interlocked.Read(ref _markerDetectionCount);
        
        _logger.LogInformation(
            "[MarkerDetection Final Stats] Total LaserScans: {LaserCount}, Total Marker Poses: {MarkerCount}",
            laserCount, markerCount);
    }

    /// <summary>
    /// Enable or disable marker detection
    /// </summary>
    public void SetEnableDetection(bool enable)
    {
        lock (_lock)
        {
            if (!_isInitialized || _markerDetectionClient == null || !_markerDetectionClient.IsInitialized)
            {
                _logger.LogWarning("Cannot set detection enable/disable: MarkerDetectionIntegrationService not initialized");
                return;
            }

            try
            {
                _markerDetectionClient.SetEnableDetection(enable);
                _logger.LogInformation("Marker detection {Status}", enable ? "enabled" : "disabled");
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error setting detection enable/disable: {Message}", ex.Message);
            }
        }
    }

    /// <summary>
    /// Get current marker pose
    /// </summary>
    public MarkerDetectionPoseStamped? GetMarkerPose()
    {
        lock (_lock)
        {
            if (!_isInitialized || _markerDetectionClient == null || !_markerDetectionClient.IsInitialized)
            {
                _logger.LogWarning("Cannot get marker pose: MarkerDetectionIntegrationService not initialized");
                return null;
            }

            try
            {
                return _markerDetectionClient.GetMarkerPose();
            }
            catch (Exception ex)
            {
                _logger.LogError(ex, "Error getting marker pose: {Message}", ex.Message);
                return null;
            }
        }
    }

    public void Dispose()
    {
        if (_disposed)
            return;

        _disposed = true;

        try
        {
            lock (_lock)
            {
                _isInitialized = false;
            }
            
            // Give loop time to stop
            Thread.Sleep(500);
            
            _updateTimer?.Dispose();
            _markerDetectionClient?.Dispose();
            _dispatchSemaphore?.Dispose();
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error disposing MarkerDetectionIntegrationService: {Message}", ex.Message);
        }
    }
}

/// <summary>
/// Marker detection type
/// </summary>
public enum MarkerDetectionType
{
    Rectangle,
    Segment
}

/// <summary>
/// Configuration for MarkerDetectionIntegrationService
/// </summary>
public class MarkerDetectionIntegrationConfiguration
{
    /// <summary>
    /// Enable/disable the integration service
    /// </summary>
    public bool Enabled { get; set; } = false;

    /// <summary>
    /// Type of marker to detect (Rectangle or Segment)
    /// </summary>
    public MarkerDetectionType MarkerType { get; set; } = MarkerDetectionType.Rectangle;

    /// <summary>
    /// Rectangle marker detection options
    /// Required if MarkerType is Rectangle
    /// </summary>
    public MarkerDetectionRectangleOptions? RectangleMarkerOptions { get; set; } = null;

    /// <summary>
    /// Segment marker detection options
    /// Required if MarkerType is Segment
    /// </summary>
    public MarkerDetectionSegmentOptions? SegmentMarkerOptions { get; set; } = null;

    /// <summary>
    /// LIDAR device ID for laser scan dispatch
    /// </summary>
    public string LidarDeviceId { get; set; } = "scan_1";

    /// <summary>
    /// Sensor ID for laser scan
    /// </summary>
    public string? LaserScanSensorId { get; set; } = "scan_1";

    /// <summary>
    /// Frame ID for laser scan
    /// </summary>
    public string? LaserScanFrameId { get; set; } = "scan_1";

    /// <summary>
    /// Laser scan data dispatch interval in milliseconds (default: 50ms = 20Hz)
    /// </summary>
    public int LaserScanDispatchIntervalMs { get; set; } = 50;

    /// <summary>
    /// Enable estimated marker pose for improved stability
    /// </summary>
    public bool EnableEstimateMarkerPose { get; set; } = false;

    /// <summary>
    /// Initial estimated marker pose X (meters)
    /// </summary>
    public double EstimateMarkerPoseX { get; set; } = 0.0;

    /// <summary>
    /// Initial estimated marker pose Y (meters)
    /// </summary>
    public double EstimateMarkerPoseY { get; set; } = 0.0;

    /// <summary>
    /// Initial estimated marker pose theta (radians)
    /// </summary>
    public double EstimateMarkerPoseTheta { get; set; } = 0.0;
}