using RobotNet10.RobotApp.TF3;
using RobotNet10.RobotApp.Xloc;
using RobotNet10.RobotApp.Navigation;

namespace RobotNet10.RobotApp.Examples;

/// <summary>
/// Example: How to properly initialize TF3BufferManager, XlocClient, and NavigationClient
/// 
/// This demonstrates the correct initialization sequence:
/// 1. Create and initialize TF3BufferManager
/// 2. Get the TF3 buffer
/// 3. Pass it to XlocClient and NavigationClient (both use same buffer)
/// 4. Proper cleanup on shutdown
/// </summary>
public class TF3XlocInitializationExample
{
    private readonly ILogger<TF3XlocInitializationExample> _logger;
    private TF3BufferManager? _tf3Manager;
    private XlocClient? _xlocClient;
    private NavigationClient? _navigationClient;

    public TF3XlocInitializationExample(ILogger<TF3XlocInitializationExample> logger)
    {
        _logger = logger;
    }

    /// <summary>
    /// Initialize all required components in the correct order
    /// </summary>
    public async Task StartupAsync()
    {
        try
        {
            _logger.LogInformation("═══════════════════════════════════════════════════════════");
            _logger.LogInformation("Starting RobotApp with TF3 and XLOC");
            _logger.LogInformation("═══════════════════════════════════════════════════════════");

            // ========================================================================
            // STEP 1: Initialize TF3BufferManager (MUST be first!)
            // ========================================================================
            _logger.LogInformation("[1/3] Initializing TF3BufferManager...");
            _logger.LogInformation("      → Creating TF3 buffer with 10-second cache");
            _logger.LogInformation("      → Publishing static robot transforms");

            _tf3Manager = new TF3BufferManager(_logger);
            _tf3Manager.Initialize();

            _logger.LogInformation("✓ TF3BufferManager initialized successfully");
            _logger.LogInformation("  Available frames: base_link, imu, scan_1, scan_2, scan_3");

            // ========================================================================
            // STEP 2: Initialize XlocClient with TF3 buffer
            // ========================================================================
            _logger.LogInformation("[2/4] Initializing XlocClient...");
            _logger.LogInformation("      → Creating XLOC instance");
            _logger.LogInformation("      → Using pre-initialized TF3 buffer");

            _xlocClient = new XlocClient(_tf3Manager.TfBuffer, _logger);
            _xlocClient.Initialize();

            _logger.LogInformation("✓ XlocClient initialized successfully");
            _logger.LogInformation("  Ready to load maps and perform localization/mapping");

            // ========================================================================
            // STEP 3: Initialize NavigationClient with same TF3 buffer
            // ========================================================================
            _logger.LogInformation("[3/4] Initializing NavigationClient...");
            _logger.LogInformation("      → Creating Navigation instance");
            _logger.LogInformation("      → Using same TF3 buffer as XlocClient");

            _navigationClient = new NavigationClient(_tf3Manager.TfBuffer, _logger);
            _navigationClient.Initialize();

            _logger.LogInformation("✓ NavigationClient initialized successfully");
            _logger.LogInformation("  Both XLOC and Navigation use the same TF3 buffer");

            // ========================================================================
            // STEP 4 (Optional): Activate map and start localization
            // ========================================================================
            _logger.LogInformation("[4/4] Loading map...");

            string mapPath = "path/to/your/map.pgm";
            if (_xlocClient.ActivateMap(mapPath))
            {
                _logger.LogInformation("✓ Map loaded: {MapPath}", mapPath);

                // Start localization
                if (_xlocClient.StartLocalization())
                {
                    _logger.LogInformation("✓ Localization started");
                }
                else
                {
                    _logger.LogWarning("⚠ Failed to start localization");
                }
            }
            else
            {
                _logger.LogWarning("⚠ Failed to load map: {MapPath}", mapPath);
            }

            _logger.LogInformation("═══════════════════════════════════════════════════════════");
            _logger.LogInformation("✓ RobotApp startup complete");
            _logger.LogInformation("═══════════════════════════════════════════════════════════");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "✗ Error during startup");
            await ShutdownAsync();
            throw;
        }
    }

    /// <summary>
    /// Demonstrate XlocClient usage
    /// </summary>
    public void DemonstrateUsage()
    {
        if (_xlocClient == null)
        {
            _logger.LogError("XlocClient not initialized");
            return;
        }

        try
        {
            _logger.LogInformation("═══════════════════════════════════════════════════════════");
            _logger.LogInformation("XlocClient Usage Examples");
            _logger.LogInformation("═══════════════════════════════════════════════════════════");

            // Get current pose
            var pose = _xlocClient.GetCurrentPose();
            if (pose.HasValue)
            {
                _logger.LogInformation("Current Pose:");
                _logger.LogInformation("  Position: ({X:F3}, {Y:F3}, {Z:F3})",
                    pose.Value.x, pose.Value.y, pose.Value.z);
                _logger.LogInformation("  Orientation: ({Qx:F3}, {Qy:F3}, {Qz:F3}, {Qw:F3})",
                    pose.Value.qx, pose.Value.qy, pose.Value.qz, pose.Value.qw);
            }

            // Get diagnostics
            var diag = _xlocClient.GetDiagnostics();
            if (diag != null)
            {
                _logger.LogInformation("Diagnostics:");
                _logger.LogInformation("  State: {State}", diag.StateString);
                _logger.LogInformation("  Reliability: {Reliability:P2}", diag.Reliability);
                _logger.LogInformation("  Matching Score: {Score:F3}", diag.MatchingScore);
            }

            // Get static map
            var staticMap = _xlocClient.GetStaticGridMap();
            if (staticMap != null)
            {
                _logger.LogInformation("Static Grid Map:");
                _logger.LogInformation("  Resolution: {Resolution:F3} m/cell", staticMap.Resolution);
                _logger.LogInformation("  Size: {Width}x{Height} cells", staticMap.Width, staticMap.Height);
            }

            _logger.LogInformation("═══════════════════════════════════════════════════════════");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Error during demonstration");
        }
    }

    /// <summary>
    /// Clean up all resources on shutdown
    /// IMPORTANT: This is the reverse order of initialization
    /// </summary>
    public async Task ShutdownAsync()
    {
        try
        {
            _logger.LogInformation("═══════════════════════════════════════════════════════════");
            _logger.LogInformation("Shutting down RobotApp");
            _logger.LogInformation("═══════════════════════════════════════════════════════════");

            // Stop any active operations
            if (_xlocClient?.IsInitialized ?? false)
            {
                try
                {
                    _logger.LogInformation("Stopping localization...");
                    _xlocClient.StopLocalization();
                    _logger.LogInformation("✓ Localization stopped");
                }
                catch (Exception ex)
                {
                    _logger.LogWarning(ex, "Error stopping localization");
                }
            }

            // Shutdown in reverse order of startup
            // First: NavigationClient
            if (_navigationClient != null)
            {
                _logger.LogInformation("Disposing NavigationClient...");
                _navigationClient.Dispose();
                _navigationClient = null;
                _logger.LogInformation("✓ NavigationClient disposed");
            }

            // Second: XlocClient
            if (_xlocClient != null)
            {
                _logger.LogInformation("Disposing XlocClient...");
                _xlocClient.Dispose();
                _xlocClient = null;
                _logger.LogInformation("✓ XlocClient disposed");
            }

            // Third: TF3BufferManager
            if (_tf3Manager != null)
            {
                _logger.LogInformation("Disposing TF3BufferManager...");
                _tf3Manager.Dispose();
                _tf3Manager = null;
                _logger.LogInformation("✓ TF3BufferManager disposed");
            }

            _logger.LogInformation("═══════════════════════════════════════════════════════════");
            _logger.LogInformation("✓ Shutdown complete");
            _logger.LogInformation("═══════════════════════════════════════════════════════════");

            await Task.CompletedTask;
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "✗ Error during shutdown");
        }
    }
}

/// <summary>
/// Example: Using TF3BufferManager with multiple components
/// </summary>
public class MultiComponentExample
{
    private readonly ILogger<MultiComponentExample> _logger;
    private TF3BufferManager? _tf3Manager;
    private XlocClient? _xlocClient;
    private NavigationClient? _navigationClient;

    public MultiComponentExample(ILogger<MultiComponentExample> logger)
    {
        _logger = logger;
    }

    /// <summary>
    /// Initialize TF3 once, then pass it to multiple components
    /// </summary>
    public async Task InitializeAsync()
    {
        try
        {
            _logger.LogInformation("Initializing components with shared TF3 buffer...");

            // 1. Initialize TF3 (shared by all components)
            _tf3Manager = new TF3BufferManager(_logger);
            _tf3Manager.Initialize();
            _logger.LogInformation("✓ TF3 initialized");

            IntPtr tfBuffer = _tf3Manager.TfBuffer;

            // 2. Initialize components that use TF3
            _xlocClient = new XlocClient(tfBuffer, _logger);
            _xlocClient.Initialize();
            _logger.LogInformation("✓ XlocClient initialized");

            // Initialize NavigationClient with same buffer
            _navigationClient = new NavigationClient(tfBuffer, _logger);
            _navigationClient.Initialize();
            _logger.LogInformation("✓ NavigationClient initialized");

            // Both components are now using the same TF3 buffer
            _logger.LogInformation("✓ All components share the same TF3 buffer");
            // _markerDetectionManager.Initialize();
            // _logger.LogInformation("✓ MarkerDetectionManager initialized");

            _logger.LogInformation("✓ All components initialized with shared TF3");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Failed to initialize components");
            throw;
        }
    }

    /// <summary>
    /// All components share the same TF3 buffer transforms
    /// </summary>
    public void PublishCustomTransform()
    {
        if (_tf3Manager?.IsInitialized != true)
        {
            _logger.LogWarning("TF3 not initialized");
            return;
        }

        try
        {
            // All components benefit from any new transforms published to TF3
            var customTransform = new TF3.TF3_Transform
            {
                timestamp_sec = (long)DateTimeOffset.UtcNow.ToUnixTimeSeconds(),
                timestamp_nsec = 0,
                frame_id = "base_link",
                child_frame_id = "payload_sensor",
                translation_x = 0.3,
                translation_y = 0.0,
                translation_z = 0.15,
                rotation_x = 0.0,
                rotation_y = 0.0,
                rotation_z = 0.0,
                rotation_w = 1.0
            };

            _tf3Manager.PublishTransform(customTransform, isStatic: true);
            _logger.LogInformation("✓ Custom transform published - available to all components");
        }
        catch (Exception ex)
        {
            _logger.LogError(ex, "Failed to publish custom transform");
        }
    }

    public async Task ShutdownAsync()
    {
        _navigationClient?.Dispose();
        _xlocClient?.Dispose();
        _tf3Manager?.Dispose();
        await Task.CompletedTask;
    }
}