using System.Runtime.InteropServices;

namespace RobotNet10.RobotApp.Xloc;

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

    #region Creation / Destruction

    /// <summary>
    /// Create an xloc instance
    /// </summary>
    /// <param name="tfBuffer">TF3 buffer core (pass IntPtr.Zero if not using TF)</param>
    /// <returns>Handle to xloc instance</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern IntPtr xloc_create(IntPtr tfBuffer);

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

    #endregion

    #region Sensor Data Dispatch

    /// <summary>
    /// Dispatch laser scan data to xloc
    /// </summary>
    /// <param name="handle">Handle to xloc 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 xloc_dispatch_laserscan(
        IntPtr handle,
        [MarshalAs(UnmanagedType.LPUTF8Str)] string sensorId,
        ref xloc_laserscan_t scan);

    /// <summary>
    /// Dispatch IMU data to xloc
    /// </summary>
    /// <param name="handle">Handle to xloc instance</param>
    /// <param name="sensorId">Sensor ID (null-terminated string)</param>
    /// <param name="imu">Pointer to IMU struct</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void xloc_dispatch_imu(
        IntPtr handle,
        [MarshalAs(UnmanagedType.LPUTF8Str)] string sensorId,
        ref xloc_imu_t imu);

    /// <summary>
    /// Dispatch odometry data to xloc
    /// </summary>
    /// <param name="handle">Handle to xloc instance</param>
    /// <param name="sensorId">Sensor ID (null-terminated string)</param>
    /// <param name="odom">Pointer to odometry struct</param>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void xloc_dispatch_odometry(
        IntPtr handle,
        [MarshalAs(UnmanagedType.LPUTF8Str)] string sensorId,
        ref xloc_odometry_t odom);

    #endregion

    #region Localization Control

    /// <summary>
    /// Activate a map
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern xloc_status_response_t xloc_activate_map(
        IntPtr handle,
        [MarshalAs(UnmanagedType.LPUTF8Str)] string mapFileName);

    /// <summary>
    /// Switch to a different map
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern xloc_status_response_t xloc_switch_map(
        IntPtr handle,
        [MarshalAs(UnmanagedType.LPUTF8Str)] string mapFileName,
        int useInitialPose,
        ref xloc_pose_t initialPose);

    /// <summary>
    /// Start localization
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern xloc_status_response_t xloc_start_localization(IntPtr handle);

    /// <summary>
    /// Stop localization
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern xloc_status_response_t xloc_stop_localization(IntPtr handle);

    /// <summary>
    /// Start mapping
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern xloc_status_response_t xloc_start_mapping(IntPtr handle);

    /// <summary>
    /// Stop mapping and optionally save the map
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern xloc_status_response_t xloc_stop_mapping(
        IntPtr handle,
        [MarshalAs(UnmanagedType.LPUTF8Str)] string mapFileName);

    /// <summary>
    /// Set initial pose
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern xloc_status_response_t xloc_set_initial_pose(
        IntPtr handle,
        ref xloc_pose_t initialPose);

    /// <summary>
    /// Change map origin
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern xloc_status_response_t xloc_change_map_origin(
        IntPtr handle,
        ref xloc_pose_t newMapOrigin);

    /// <summary>
    /// Start updating existing map
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern xloc_status_response_t xloc_start_update_map(IntPtr handle);

    /// <summary>
    /// Stop updating map and optionally save
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern xloc_status_response_t xloc_stop_update_map(
        IntPtr handle,
        int saveUpdatedMap);

    /// <summary>
    /// Reset SLAM error state
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern xloc_status_response_t xloc_reset_slam_error(IntPtr handle);

    #endregion

    #region Getters

    /// <summary>
    /// Get current pose estimate
    /// </summary>
    /// <returns>Pointer to pose (must be freed with xloc_free_pose)</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern IntPtr xloc_get_current_pose(IntPtr handle);

    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern IntPtr xloc_get_diagnostics(IntPtr handle);

    /// <summary>
    /// Get static grid map (from loaded map file)
    /// </summary>
    /// <param name="handle">Handle to xloc instance</param>
    /// <param name="reloadFromFile">If non-zero, reload map from file</param>
    /// <returns>Pointer to occupancy grid (must be freed with xloc_free_occupancy_grid)</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern IntPtr xloc_get_static_grid_map(IntPtr handle, int reloadFromFile);

    /// <summary>
    /// Get online grid map (from SLAM)
    /// </summary>
    /// <param name="handle">Handle to xloc instance</param>
    /// <returns>Pointer to occupancy grid (must be freed with xloc_free_occupancy_grid)</returns>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern IntPtr xloc_get_online_grid_map(IntPtr handle);

    #endregion

    #region Memory Management

    /// <summary>
    /// Free status response
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void xloc_free_status_response(ref xloc_status_response_t response);

    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void xloc_free_diagnostics(IntPtr diagnostics);

    /// <summary>
    /// Free C string allocated by xloc
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void xloc_free_cstring(IntPtr str);

    /// <summary>
    /// Free pose allocated by xloc
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void xloc_free_pose(IntPtr pose);

    /// <summary>
    /// Free occupancy grid allocated by xloc
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void xloc_free_occupancy_grid(IntPtr grid);

    #endregion
}