using Microsoft.Extensions.Logging;
using RobotNet10.Shared.Sensor;
using System.Runtime.InteropServices;

namespace RobotNet10.RobotApp.MarkerDetection;

/// <summary>
/// High-level wrapper for marker_detection library
/// Provides thread-safe access to marker detection functionality with automatic resource management
/// </summary>
public class MarkerDetectionClient : IDisposable
{
    private IntPtr _handle = IntPtr.Zero;
    private IntPtr _tfBuffer = IntPtr.Zero;  // TF3 buffer (optional)
    private readonly object _lock = new object();
    private bool _disposed = false;
    private readonly ILogger? _logger;
    public MarkerDetectionClient(ILogger? logger = null)
    {
        _logger = logger;
    }

    /// <summary>
    /// Initialize marker detection instance with rectangle marker options
    /// </summary>
    /// <param name="options">Rectangle marker options</param>
    /// <param name="tfBuffer">Optional TF3 buffer (pass IntPtr.Zero if not using TF)</param>
    public void InitializeRectangle(MarkerDetectionRectangleOptions options, IntPtr tfBuffer = default)
    {
        lock (_lock)
        {
            if (_handle != IntPtr.Zero)
            {
                _logger?.LogWarning("MarkerDetectionClient already initialized");
                return;
            }

            try
            {
                _tfBuffer = tfBuffer;

                // Convert options to C struct
                var mdOptions = new md_rectangle_marker_options_t
                {
                    marker_frame_id = string.IsNullOrEmpty(options.MarkerFrameId)
                        ? IntPtr.Zero
                        : Marshal.StringToHGlobalAnsi(options.MarkerFrameId),
                    marker_length = options.MarkerLength,
                    marker_width = options.MarkerWidth,
                    marker_geometric_error = options.MarkerGeometricError,
                    marker_intensity_threshold_ = options.MarkerIntensityThreshold,
                    angle_min_of_scan = options.AngleMinOfScan,
                    angle_max_of_scan = options.AngleMaxOfScan,
                    min_range_of_scan = options.MinRangeOfScan,
                    max_range_of_scan = options.MaxRangeOfScan,
                    use_estimate_marker_pose = options.UseEstimateMarkerPose ? 1 : 0,
                    max_position_error = options.MaxPositionError,
                    max_yaw_angle_error = options.MaxYawAngleError,
                    num_filter_samples = options.NumFilterSamples,
                    leg_thickness_offset = options.LegThicknessOffset,
                    run_debug = options.RunDebug ? 1 : 0
                };

                _handle = MarkerDetectionNativeInterface.marker_detection_create_rectangle_md(ref mdOptions, _tfBuffer);
                // Free allocated string
                if (mdOptions.marker_frame_id != IntPtr.Zero)
                {
                    Marshal.FreeHGlobal(mdOptions.marker_frame_id);
                }

                if (_handle == IntPtr.Zero)
                {
                    throw new InvalidOperationException("Failed to create marker detection instance. marker_detection_create_rectangle_md returned null.");
                }

                _logger?.LogInformation("MarkerDetectionClient initialized successfully with rectangle marker (handle: {Handle})", _handle);
            }
            catch (Exception ex)
            {
                _logger?.LogError(ex, "Exception in InitializeRectangle: {Message}", ex.Message);
                throw;
            }
        }
    }

    /// <summary>
    /// Initialize marker detection instance with segment marker options
    /// </summary>
    /// <param name="options">Segment marker options</param>
    /// <param name="tfBuffer">Optional TF3 buffer (pass IntPtr.Zero if not using TF)</param>
    public void InitializeSegment(MarkerDetectionSegmentOptions options, IntPtr tfBuffer = default)
    {
        lock (_lock)
        {
            if (_handle != IntPtr.Zero)
            {
                _logger?.LogWarning("MarkerDetectionClient already initialized");
                return;
            }

            try
            {
                _tfBuffer = tfBuffer;

                // Convert options to C struct
                var mdOptions = new md_segment_marker_options_t
                {
                    marker_frame_id = string.IsNullOrEmpty(options.MarkerFrameId)
                        ? IntPtr.Zero
                        : Marshal.StringToHGlobalAnsi(options.MarkerFrameId),
                    marker_length = options.MarkerLength,
                    marker_geometric_error = options.MarkerGeometricError,
                    marker_intensity_threshold_ = options.MarkerIntensityThreshold,
                    angle_min_of_scan = options.AngleMinOfScan,
                    angle_max_of_scan = options.AngleMaxOfScan,
                    min_range_of_scan = options.MinRangeOfScan,
                    max_range_of_scan = options.MaxRangeOfScan,
                    use_estimate_marker_pose = options.UseEstimateMarkerPose ? 1 : 0,
                    max_position_error = options.MaxPositionError,
                    max_yaw_angle_error = options.MaxYawAngleError,
                    num_filter_samples = options.NumFilterSamples,
                    leg_thickness_offset = options.LegThicknessOffset,
                    run_debug = options.RunDebug ? 1 : 0
                };

                _handle = MarkerDetectionNativeInterface.marker_detection_create_segment_md(ref mdOptions, _tfBuffer);

                // Free allocated string
                if (mdOptions.marker_frame_id != IntPtr.Zero)
                {
                    Marshal.FreeHGlobal(mdOptions.marker_frame_id);
                }

                if (_handle == IntPtr.Zero)
                {
                    throw new InvalidOperationException("Failed to create marker detection instance. marker_detection_create_segment_md returned null.");
                }

                _logger?.LogInformation("MarkerDetectionClient initialized successfully with segment marker (handle: {Handle})", _handle);
            }
            catch (Exception ex)
            {
                _logger?.LogError(ex, "Exception in InitializeSegment: {Message}", ex.Message);
                throw;
            }
        }
    }

    /// <summary>
    /// Check if marker detection is initialized
    /// </summary>
    public bool IsInitialized
    {
        get
        {
            lock (_lock)
            {
                return _handle != IntPtr.Zero;
            }
        }
    }

    /// <summary>
    /// Enable or disable marker detection
    /// </summary>
    /// <param name="enable">True to enable detection, false to disable</param>
    public void SetEnableDetection(bool enable)
    {
        ThrowIfNotInitialized();
        try{
            lock (_lock)
            {
                // Console.WriteLine($"[MarkerDetectionClient] Setting enable detection to {enable}");
                MarkerDetectionNativeInterface.marker_detection_set_enable_detection(_handle, enable ? 1 : 0);
                // Console.WriteLine($"[MarkerDetectionClient] Finish !!!");
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Exception in SetEnableDetection: {Message}", ex.Message);
            throw;
        }
        _logger?.LogDebug("Marker detection {Status}", enable ? "enabled" : "disabled");
    }

    /// <summary>
    /// Set estimated marker pose to improve stability
    /// The pose is the position and orientation of the marker (cart or station) 
    /// relative to the laser scan sensor coordinate system
    /// </summary>
    /// <param name="x">X position in meters</param>
    /// <param name="y">Y position in meters</param>
    /// <param name="theta">Orientation angle in radians</param>
    public void SetEstimateMarkerPose(double x, double y, double theta)
    {
        ThrowIfNotInitialized();

        var pose = new md_pose2d_t
        {
            x = x,
            y = y,
            theta = theta
        };

        lock (_lock)
        {
            MarkerDetectionNativeInterface.marker_detection_set_estimate_marker_pose(_handle, ref pose);
        }

        _logger?.LogDebug("Set estimated marker pose: x={X:F3}, y={Y:F3}, theta={Theta:F3}", x, y, theta);
    }

    /// <summary>
    /// Dispatch laser scan data to marker detection
    /// </summary>
    /// <param name="scan">Laser scan data</param>
    /// <param name="sensorId">Sensor ID</param>
    public void DispatchLaserScan(LaserScan scan, string sensorId)
    {
        ThrowIfNotInitialized();

        md_laserscan_t mdScan = default;
        try
        {
            _logger?.LogTrace("[DISPATCH-START] LaserScan from {SensorId}, ranges={RangeCount}",
                sensorId, scan.Ranges?.Length ?? 0);

            mdScan = scan.ToMarkerDetectionLaserScan();

            lock (_lock)
            {
                // _logger?.LogTrace("[NATIVE-CALL] marker_detection_dispatch_laserscan...");
                // in ra dữ liệu trong mảng intensities và ranges trên cùng một dòng
                // Console.WriteLine($"[MarkerDetectionClient] Dispatching LaserScan - Ranges: [{string.Join(", ", scan.Ranges ?? new float[0])}], Intensities: [{string.Join(", ", scan.Intensities ?? new float[0])}]");
                MarkerDetectionNativeInterface.marker_detection_dispatch_laserscan(_handle, sensorId, ref mdScan);
            //     _logger?.LogInformation("[NATIVE-DONE] marker_detection_dispatch_laserscan OK");
            }

            _logger?.LogTrace("[DISPATCH-DONE] LaserScan from {SensorId}", sensorId);
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[DISPATCH-ERROR] LaserScan dispatch failed: {Message}", ex.Message);
            throw;
        }
        finally
        {
            // Free allocated memory
            MarkerDetectionConversionExtensions.FreeMarkerDetectionLaserScan(ref mdScan);
        }
    }

    /// <summary>
    /// Get current marker pose estimate
    /// Returns the marker coordinates in the laser scan sensor coordinate system
    /// </summary>
    /// <returns>Pose stamped with header and pose, or null if no pose available</returns>
    public MarkerDetectionPoseStamped? GetMarkerPose()
    {
        ThrowIfNotInitialized();

        IntPtr posePtr = IntPtr.Zero;
        try
        {
            _logger?.LogTrace("[DEBUG] About to call marker_detection_get_marker_pose()...");
            lock (_lock)
            {
                posePtr = MarkerDetectionNativeInterface.marker_detection_get_marker_pose(_handle);
            }
            _logger?.LogTrace("[DEBUG] marker_detection_get_marker_pose() returned: {PosePtr}", posePtr);

            if (posePtr == IntPtr.Zero)
            {
                _logger?.LogTrace("marker_detection_get_marker_pose returned null (no pose available)");
                return null;
            }

            // _logger?.LogTrace("[DEBUG] Marshaling pose_stamped structure...");
            // Marshal the pose_stamped from unmanaged memory
            var poseStamped = Marshal.PtrToStructure<md_pose_stamped_t>(posePtr);
            // _logger?.LogTrace("[DEBUG] Pose_stamped marshaled successfully");

            // Convert to managed type
            var result = new MarkerDetectionPoseStamped
            {
                Header = new RobotNet10.Shared.Header
                {
                    Seq = poseStamped.header.seq,
                    Stamp = poseStamped.header.stamp.ToDateTime(),
                    FrameId = poseStamped.header.frame_id != IntPtr.Zero
                        ? Marshal.PtrToStringAnsi(poseStamped.header.frame_id) ?? string.Empty
                        : string.Empty
                },
                Pose = new MarkerDetectionPose
                {
                    Position = new double[]
                    {
                        poseStamped.pose.position[0],
                        poseStamped.pose.position[1],
                        poseStamped.pose.position[2]
                    },
                    Orientation = new double[]
                    {
                        poseStamped.pose.orientation[0],
                        poseStamped.pose.orientation[1],
                        poseStamped.pose.orientation[2],
                        poseStamped.pose.orientation[3]
                    }
                }
            };
            //In ra dữ liệu của marker pose 
            // Console.WriteLine($"[DEBUG] Marker Pose - Position: x={result.Pose.Position[0]:F3}, y={result.Pose.Position[1]:F3}, z={result.Pose.Position[2]:F3}");
            // Console.WriteLine($"[DEBUG] Marker Pose - Orientation: x={result.Pose.Orientation[0]:F3}, y={result.Pose.Orientation[1]:F3}, z={result.Pose.Orientation[2]:F3}, w={result.Pose.Orientation[3]:F3}");
            return result;
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "[DEBUG] Exception in GetMarkerPose: {Message}", ex.Message);
            return null;
        }
        finally
        {
            if (posePtr != IntPtr.Zero)
            {
                MarkerDetectionNativeInterface.marker_detection_free_pose_stamped(posePtr);
            }
        }
    }

    private void ThrowIfNotInitialized()
    {
        if (!IsInitialized)
        {
            throw new InvalidOperationException("MarkerDetectionClient is not initialized. Call InitializeRectangle or InitializeSegment first.");
        }
    }

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

        _disposed = true;

        lock (_lock)
        {
            if (_handle != IntPtr.Zero)
            {
                try
                {
                    MarkerDetectionNativeInterface.marker_detection_destroy(_handle);
                    _logger?.LogInformation("MarkerDetectionClient disposed successfully");
                }
                catch (Exception ex)
                {
                    _logger?.LogError(ex, "Error disposing MarkerDetectionClient");
                }
                finally
                {
                    _handle = IntPtr.Zero;
                }
            }
        }
    }
}

/// <summary>
/// Helper extension to convert md_unix_time_t to DateTime
/// </summary>
internal static class MarkerDetectionTimeExtensions
{
    public static DateTime ToDateTime(this md_unix_time_t unixTime)
    {
        long totalNanoseconds = (long)unixTime.sec * 1_000_000_000 + unixTime.nsec;
        long ticks = totalNanoseconds / 100 + DateTime.UnixEpoch.Ticks;
        return new DateTime(ticks, DateTimeKind.Utc);
    }
}

/// <summary>
/// Marker detection pose stamped (managed wrapper)
/// </summary>
public class MarkerDetectionPoseStamped
{
    public RobotNet10.Shared.Header Header { get; set; } = new();
    public MarkerDetectionPose Pose { get; set; } = new();
}

/// <summary>
/// Marker detection pose (managed wrapper)
/// </summary>
public class MarkerDetectionPose
{
    public double[] Position { get; set; } = new double[3]; // x, y, z
    public double[] Orientation { get; set; } = new double[4]; // x, y, z, w (quaternion)
}

/// <summary>
/// Rectangle marker detection options
/// </summary>
public class MarkerDetectionRectangleOptions
{
    public string MarkerFrameId { get; set; } = string.Empty;
    public double MarkerLength { get; set; } = 0.0;
    public double MarkerWidth { get; set; } = 0.0;
    public double MarkerGeometricError { get; set; } = 0.0;
    public double MarkerIntensityThreshold { get; set; } = 0.0;
    public double AngleMinOfScan { get; set; } = 0.0;
    public double AngleMaxOfScan { get; set; } = 0.0;
    public double MinRangeOfScan { get; set; } = 0.0;
    public double MaxRangeOfScan { get; set; } = 100.0;
    public bool UseEstimateMarkerPose { get; set; } = false;
    public double MaxPositionError { get; set; } = 0.0;
    public double MaxYawAngleError { get; set; } = 0.0;
    public ushort NumFilterSamples { get; set; } = 0;
    public double LegThicknessOffset { get; set; } = 0.0;
    public bool RunDebug { get; set; } = false;
}

/// <summary>
/// Segment marker detection options
/// </summary>
public class MarkerDetectionSegmentOptions
{
    public string MarkerFrameId { get; set; } = string.Empty;
    public double MarkerLength { get; set; } = 0.0;
    public double MarkerGeometricError { get; set; } = 0.0;
    public double MarkerIntensityThreshold { get; set; } = 0.0;
    public double AngleMinOfScan { get; set; } = 0.0;
    public double AngleMaxOfScan { get; set; } = 0.0;
    public double MinRangeOfScan { get; set; } = 0.0;
    public double MaxRangeOfScan { get; set; } = 100.0;
    public bool UseEstimateMarkerPose { get; set; } = false;
    public double MaxPositionError { get; set; } = 0.0;
    public double MaxYawAngleError { get; set; } = 0.0;
    public ushort NumFilterSamples { get; set; } = 0;
    public double LegThicknessOffset { get; set; } = 0.0;
    public bool RunDebug { get; set; } = false;
}