using RobotNet10.RobotApp.TF3;
using Microsoft.Extensions.Hosting;

namespace RobotNet10.RobotApp.TF3;

/// <summary>
/// Manages TF3 buffer initialization and static transform publishing.
/// This service MUST be initialized first before other components that depend on TF3.
/// Implements IHostedService to ensure it initializes when the application starts.
/// The TF3 buffer can then be passed to XlocClient, navigation, and other components.
/// </summary>
public class TF3BufferManager : IHostedService, IDisposable
{
    private IntPtr _tfBuffer = IntPtr.Zero;
    private readonly object _lock = new object();
    private bool _disposed = false;
    private readonly ILogger? _logger;
    private bool _initialized = false;

    /// <summary>
    /// Get the TF3 buffer pointer for use in other components
    /// </summary>
    public IntPtr TfBuffer
    {
        get
        {
            lock (_lock)
            {
                if (_tfBuffer == IntPtr.Zero)
                {
                    throw new InvalidOperationException("TF3 buffer not initialized. Call Initialize() first.");
                }
                return _tfBuffer;
            }
        }
    }

    /// <summary>
    /// Check if TF3 buffer is initialized
    /// </summary>
    public bool IsInitialized
    {
        get
        {
            lock (_lock)
            {
                return _initialized;
            }
        }
    }

    /// <summary>
    /// Initialize TF3BufferManager with optional logger
    /// </summary>
    public TF3BufferManager(ILogger? logger = null)
    {
        _logger = logger;
    }

    /// <summary>
    /// Initialize TF3 buffer and publish static transforms
    /// Should be called once at startup, before initializing XlocClient or navigation components
    /// </summary>
    public void Initialize()
    {
        lock (_lock)
        {
            if (_initialized)
            {
                _logger?.LogWarning("TF3BufferManager already initialized");
                return;
            }

            try
            {
                _logger?.LogInformation("[TF3] Initializing TF3 buffer manager...");

                // Create TF3 buffer (10 second cache)
                _tfBuffer = TF3NativeInterface.tf3_buffer_create(10);

                if (_tfBuffer == IntPtr.Zero)
                {
                    throw new InvalidOperationException("Failed to create TF3 buffer");
                }

                _logger?.LogInformation("[TF3] TF3 buffer created successfully: {Buffer}", _tfBuffer);

                // Publish static transforms
                PublishStaticTransforms();

                _initialized = true;
                _logger?.LogInformation("[TF3] TF3BufferManager initialized successfully");
            }
            catch (Exception ex)
            {
                _logger?.LogError(ex, "[TF3] Exception during initialization: {Message}", ex.Message);
                // Clean up on error
                if (_tfBuffer != IntPtr.Zero)
                {
                    try
                    {
                        TF3NativeInterface.tf3_buffer_destroy(_tfBuffer);
                    }
                    catch { }
                    _tfBuffer = IntPtr.Zero;
                }
                throw;
            }
        }
    }

    /// <summary>
    /// Publish static transforms to TF3 buffer.
    /// Sets up the robot's coordinate frame tree.
    /// Must be called after buffer initialization.
    /// </summary>
    private void PublishStaticTransforms()
    {
        try
        {
            // _logger?.LogInformation("[TF3] ═══════════════════════════════════════════");
            // _logger?.LogInformation("[TF3] Publishing static transforms to TF3 buffer...");
            // _logger?.LogInformation("[TF3] Buffer handle: {Buffer}", _tfBuffer);

            var now = DateTime.UtcNow;
            long sec = ((DateTimeOffset)now).ToUnixTimeSeconds();
            long nsec = (now.Ticks % TimeSpan.TicksPerSecond) * 100;

            // base_link → imu_link (IMU at robot center, colocated with base_link)
            var baseToImu = new TF3_Transform
            {
                timestamp_sec = sec,
                timestamp_nsec = nsec,
                frame_id = "base_link",
                child_frame_id = "imu",
                translation_x = 0.0,
                translation_y = 0.0,
                translation_z = 0.0,
                rotation_x = 1.0,
                rotation_y = 0.0,
                rotation_z = 0.0,
                rotation_w = 0.0
            };

            _logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}", 
                baseToImu.frame_id, baseToImu.child_frame_id);
            bool result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref baseToImu, "tf3_buffer_manager", true);

            // base_link → scan_1 (LIDAR at front, 15cm backward from base_link)
            var baseToLidar1 = new TF3_Transform
            {
                timestamp_sec = sec,
                timestamp_nsec = nsec,
                frame_id = "base_link",
                child_frame_id = "scan_1",
                translation_x = 0.2985,
                translation_y = 0.0,
                translation_z = 0.0,
                rotation_x = 0.0,
                rotation_y = 0.0,
                rotation_z = 0.0,
                rotation_w = 1.0
            };

            _logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}", 
                baseToLidar1.frame_id, baseToLidar1.child_frame_id);
            result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref baseToLidar1, "tf3_buffer_manager", true);

            // base_link → scan_2 (LIDAR 2 at front-right corner, -45° rotation)
            var baseToLidar2 = new TF3_Transform
            {
                timestamp_sec = sec,
                timestamp_nsec = nsec,
                frame_id = "base_link",
                child_frame_id = "scan_2",
                translation_x = -0.2985,
                translation_y = 0.0,
                translation_z = 0.0,
                rotation_x = 0.0,
                rotation_y = 0.0,
                rotation_z = 1.0,
                rotation_w = 0.0
            };

            _logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}", 
                baseToLidar2.frame_id, baseToLidar2.child_frame_id);
            result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref baseToLidar2, "tf3_buffer_manager", true);
  
            // base_link → scan_3 (LIDAR 3 at front-left corner, +45° rotation)
            var baseToLidar3 = new TF3_Transform
            {
                timestamp_sec = sec,
                timestamp_nsec = nsec,
                frame_id = "base_link",
                child_frame_id = "scan_3",
                translation_x = 0.325,
                translation_y = -0.325,
                translation_z = 0.0,
                rotation_x = 0.0,
                rotation_y = 0.0,
                rotation_z = 0.0,
                rotation_w = 1.0
            };

            _logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}", 
                baseToLidar3.frame_id, baseToLidar3.child_frame_id);
            result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref baseToLidar3, "tf3_buffer_manager", true);
            
            // Publish initial map → odom transform (DYNAMIC - not static!)
            // This is required for costmap to initialize and detect the 'map' frame.
            // Navigation will NOT hang waiting for this transform to exist.
            // XlocClient will UPDATE this transform when localization starts with actual pose data.
            // We publish as isStatic=false so it can be dynamically updated by XlocClient.
            // var mapToOdom = new TF3_Transform
            // {
            //     timestamp_sec = sec,
            //     timestamp_nsec = nsec,
            //     frame_id = "map",
            //     child_frame_id = "odom",
            //     translation_x = 0.0,
            //     translation_y = 0.0,
            //     translation_z = 0.0,
            //     rotation_x = 0.0,
            //     rotation_y = 0.0,
            //     rotation_z = 0.0,
            //     rotation_w = 1.0
            // };

            // _logger?.LogInformation("[TF3] Publishing INITIAL transform (dummy): {ParentFrame} → {ChildFrame} [Will be updated by XLOC]", 
            //     mapToOdom.frame_id, mapToOdom.child_frame_id);
            // result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref mapToOdom, "tf3_buffer_manager", false);
            
            // Publish odom → base_footprint frame
            // var odomToFootprint = new TF3_Transform
            // {
            //     timestamp_sec = sec,
            //     timestamp_nsec = nsec,
            //     frame_id = "odom",
            //     child_frame_id = "base_footprint",
            //     translation_x = 0.0,
            //     translation_y = 0.0,
            //     translation_z = 0.0,
            //     rotation_x = 0.0,
            //     rotation_y = 0.0,
            //     rotation_z = 0.0,
            //     rotation_w = 1.0
            // };

            // _logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}", 
            //     odomToFootprint.frame_id, odomToFootprint.child_frame_id);
            // result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref odomToFootprint, "tf3_buffer_manager", false);
        
            // var footPrintTobaseLink = new TF3_Transform
            // {
            //     timestamp_sec = sec,
            //     timestamp_nsec = nsec,
            //     frame_id = "base_footprint",
            //     child_frame_id = "base_link",
            //     translation_x = 0.0,
            //     translation_y = 0.0,
            //     translation_z = 0.0,
            //     rotation_x = 0.0,
            //     rotation_y = 0.0,
            //     rotation_z = 0.0,
            //     rotation_w = 1.0
            // };

            // _logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}", 
            //     footPrintTobaseLink.frame_id, footPrintTobaseLink.child_frame_id);
            // result = TF3NativeInterface.tf3_set_transform(_tfBuffer, ref footPrintTobaseLink, "tf3_buffer_manager", true);
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[TF3] ❌ Error publishing static transforms: {Message}", ex.Message);
            throw;
        }
    }

    /// <summary>
    /// Publish a custom transform to the TF3 buffer
    /// </summary>
    public void PublishTransform(TF3_Transform transform, bool isStatic = false)
    {
        lock (_lock)
        {
            if (!_initialized)
            {
                throw new InvalidOperationException("TF3BufferManager not initialized. Call Initialize() first.");
            }

            try
            {
                TF3NativeInterface.tf3_set_transform(_tfBuffer, ref transform, "tf3_buffer_manager", isStatic);
                _logger?.LogDebug("[TF3] Published transform: {FrameId} → {ChildFrameId}", 
                    transform.frame_id, transform.child_frame_id);
            }
            catch (Exception ex)
            {
                _logger?.LogError(ex, "[TF3] Error publishing transform: {Message}", ex.Message);
                throw;
            }
        }
    }

    /// <summary>
    /// Get a transform from the TF3 buffer
    /// </summary>
    public TF3_Transform? GetTransform(string sourceFrame, string targetFrame)
    {
        lock (_lock)
        {
            if (!_initialized)
            {
                throw new InvalidOperationException("TF3BufferManager not initialized. Call Initialize() first.");
            }

            try
            {
                // Call the TF3 native interface to get the transform
                // This is a simplified version - you may need to add the actual native method
                _logger?.LogDebug("[TF3] Querying transform: {SourceFrame} → {TargetFrame}", sourceFrame, targetFrame);
                return null;  // Return null for now - implement based on actual TF3 API
            }
            catch (Exception ex)
            {
                _logger?.LogError(ex, "[TF3] Error getting transform: {Message}", ex.Message);
                return null;
            }
        }
    }

    /// <summary>
    /// IHostedService implementation - initializes TF3 buffer when application starts
    /// This ensures TF3 is ready before XlocIntegrationService and NavigationIntegrationService
    /// </summary>
    public Task StartAsync(CancellationToken cancellationToken)
    {
        try
        {
            Initialize();
            return Task.CompletedTask;
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[TF3] Failed to start TF3BufferManager: {Message}", ex.Message);
            throw;
        }
    }

    /// <summary>
    /// IHostedService implementation - stops TF3 buffer when application shuts down
    /// </summary>
    public Task StopAsync(CancellationToken cancellationToken)
    {
        Dispose();
        return Task.CompletedTask;
    }

    public void Dispose()
    {
        Dispose(true);
        GC.SuppressFinalize(this);
    }

    protected virtual void Dispose(bool disposing)
    {
        if (_disposed)
            return;

        lock (_lock)
        {
            if (_tfBuffer != IntPtr.Zero)
            {
                try
                {
                    TF3NativeInterface.tf3_buffer_destroy(_tfBuffer);
                    _logger?.LogInformation("[TF3] TF3 buffer disposed successfully");
                }
                catch (Exception ex)
                {
                    _logger?.LogError(ex, "[TF3] Error disposing TF3 buffer: {Message}", ex.Message);
                }
                finally
                {
                    _tfBuffer = IntPtr.Zero;
                    _initialized = false;
                }
            }
        }

        _disposed = true;
    }

    ~TF3BufferManager()
    {
        Dispose(false);
    }
}