using System.Runtime.InteropServices;

namespace RobotNet10.RobotApp.MarkerDetection;

/// <summary>
/// P/Invoke declarations for marker_detection C API
/// </summary>
public static class MarkerDetectionNativeInterface
{
    // Library path - adjust if needed
    private const string LibraryPath = "/usr/local/lib/libmarker_detection.so";

    #region Creation / Destruction

    /// <summary>
    /// Create a rectangle marker detection instance
    /// </summary>
    /// <param name="options">Marker options</param>
    /// <param name="tfBuffer">TF3 buffer core (pass IntPtr.Zero if not using TF)</param>
    /// <returns>Handle to marker detection instance</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern IntPtr marker_detection_create_rectangle_md(
        ref md_rectangle_marker_options_t options,
        IntPtr tfBuffer);

    /// <summary>
    /// Create a segment marker detection instance
    /// </summary>
    /// <param name="options">Marker options</param>
    /// <param name="tfBuffer">TF3 buffer core (pass IntPtr.Zero if not using TF)</param>
    /// <returns>Handle to marker detection instance</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern IntPtr marker_detection_create_segment_md(
        ref md_segment_marker_options_t options,
        IntPtr tfBuffer);

    /// <summary>
    /// Destroy a marker detection instance
    /// </summary>
    /// <param name="handle">Handle to marker detection instance</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void marker_detection_destroy(IntPtr handle);

    #endregion

    #region Control

    /// <summary>
    /// Enable or disable marker detection
    /// </summary>
    /// <param name="handle">Handle to marker detection instance</param>
    /// <param name="enable">1 to enable, 0 to disable</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void marker_detection_set_enable_detection(IntPtr handle, int enable);

    /// <summary>
    /// Set estimated marker pose to improve stability
    /// </summary>
    /// <param name="handle">Handle to marker detection instance</param>
    /// <param name="pose">Estimated 2D pose (x, y, theta) relative to laser scan sensor frame</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void marker_detection_set_estimate_marker_pose(IntPtr handle, ref md_pose2d_t pose);

    #endregion

    #region Sensor Data Dispatch

    /// <summary>
    /// Dispatch laser scan data to marker detection
    /// </summary>
    /// <param name="handle">Handle to marker detection instance</param>
    /// <param name="sensorId">Sensor ID (null-terminated string)</param>
    /// <param name="scan">Pointer to laser scan struct</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void marker_detection_dispatch_laserscan(
        IntPtr handle,
        [MarshalAs(UnmanagedType.LPUTF8Str)] string sensorId,
        ref md_laserscan_t scan);

    #endregion

    #region Getters

    /// <summary>
    /// Get current marker pose estimate
    /// </summary>
    /// <param name="handle">Handle to marker detection instance</param>
    /// <returns>Pointer to pose_stamped (must be freed with marker_detection_free_pose_stamped) or IntPtr.Zero if no pose available</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern IntPtr marker_detection_get_marker_pose(IntPtr handle);

    #endregion

    #region Memory Management

    /// <summary>
    /// Free pose_stamped allocated by marker_detection_get_marker_pose
    /// </summary>
    /// <param name="p">Pointer to pose_stamped to free</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void marker_detection_free_pose_stamped(IntPtr p);

    /// <summary>
    /// Free C string allocated by the API
    /// </summary>
    /// <param name="s">Pointer to C string to free</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void marker_detection_free_cstring(IntPtr s);

    #endregion
}