using RobotNet10.RobotApp.TF3;
using RobotNet10.Shared.Geometry;
using Microsoft.Extensions.Configuration;
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;
    private readonly List<Tf3StaticTransformConfig> _staticTransforms;

    // Fallback used when "Tf3:StaticTransforms" is missing from configuration (e.g. Examples/ usage without IConfiguration)
    private static List<Tf3StaticTransformConfig> DefaultStaticTransforms => new()
    {
        new Tf3StaticTransformConfig
        {
            FrameId = "base_link",
            ChildFrameId = "imu",
            Pose = new Pose(new Point(0.0, 0.0, 0.0), new Quaternion(1.0, 0.0, 0.0, 0.0))
        },
        new Tf3StaticTransformConfig
        {
            FrameId = "base_link",
            ChildFrameId = "scan_1",
            Pose = new Pose(new Point(0.095, 0.01, 0.0), new Quaternion(0.0, 0.0, 0.0261769, 0.9996573))
        },
        new Tf3StaticTransformConfig
        {
            FrameId = "base_link",
            ChildFrameId = "scan_2",
            Pose = new Pose(new Point(-0.355, -0.01, 0.0), new Quaternion(0.0, 0.0, 1.0, 0.0))
        },
        new Tf3StaticTransformConfig
        {
            FrameId = "base_link",
            ChildFrameId = "scan_3",
            Pose = new Pose(new Point(0.707, 0.2825, 0.0), new Quaternion(0.0, 0.0, 0.3826834, 0.9238795))
        }
    };

    /// <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 configuration and logger.
    /// Static transforms are bound from "Tf3:StaticTransforms"; falls back to defaults if absent.
    /// </summary>
    public TF3BufferManager(IConfiguration? configuration = null, ILogger? logger = null)
    {
        _logger = logger;

        var configuredTransforms = configuration?.GetSection("Tf3:StaticTransforms")
            .Get<List<Tf3StaticTransformConfig>>();
        _staticTransforms = (configuredTransforms != null && configuredTransforms.Count > 0)
            ? configuredTransforms
            : DefaultStaticTransforms;
    }

    /// <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;

            // Static transforms configured via appsettings.json ("Tf3:StaticTransforms")
            foreach (var config in _staticTransforms)
            {
                var transform = new TF3_Transform
                {
                    timestamp_sec = sec,
                    timestamp_nsec = nsec,
                    frame_id = config.FrameId,
                    child_frame_id = config.ChildFrameId,
                    translation_x = config.Pose.Position.X,
                    translation_y = config.Pose.Position.Y,
                    translation_z = config.Pose.Position.Z,
                    rotation_x = config.Pose.Orientation.X,
                    rotation_y = config.Pose.Orientation.Y,
                    rotation_z = config.Pose.Orientation.Z,
                    rotation_w = config.Pose.Orientation.W
                };

                _logger?.LogDebug("[TF3] Publishing transform: {ParentFrame} → {ChildFrame}",
                    transform.frame_id, transform.child_frame_id);
                TF3NativeInterface.tf3_set_transform(_tfBuffer, ref transform, "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);
    }
}