using System.Runtime.InteropServices;

namespace RobotNet10.RobotApp.TF3;

/// <summary>
/// C# P/Invoke wrapper for TF3 (Transform Framework) C API
/// Provides coordinate transform management for robotics applications
/// </summary>

/// <summary>
/// Opaque handle to TF3 BufferCore (TF3_BufferCore in C API)
/// </summary>
public struct TF3_BufferCore
{
    public IntPtr Handle;
}

/// <summary>
/// Transform structure for TF3 (matches TF3_Transform in C API)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct TF3_Transform
{
    // Header
    public long timestamp_sec;
    public long timestamp_nsec;
    
    [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)]
    public string frame_id;
    
    [MarshalAs(UnmanagedType.ByValTStr, SizeConst = 256)]
    public string child_frame_id;
    
    // Translation
    public double translation_x;
    public double translation_y;
    public double translation_z;
    
    // Rotation (quaternion)
    public double rotation_x;
    public double rotation_y;
    public double rotation_z;
    public double rotation_w;
}

/// <summary>
/// TF3 error codes (matches TF3_ErrorCode in C API)
/// </summary>
public enum TF3_ErrorCode
{
    TF3_OK = 0,
    TF3_ERROR_LOOKUP = 1,
    TF3_ERROR_CONNECTIVITY = 2,
    TF3_ERROR_EXTRAPOLATION = 3,
    TF3_ERROR_INVALID_ARGUMENT = 4,
    TF3_ERROR_TIMEOUT = 5,
    TF3_ERROR_UNKNOWN = 99
}

/// <summary>
/// P/Invoke declarations for libtf3.so
/// </summary>
public static class TF3NativeInterface
{
    private const string LibraryPath = "/usr/local/lib/libtf3.so";

    /// <summary>
    /// Create TF3 buffer with cache time
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern IntPtr tf3_buffer_create(int cacheTimeSec);

    /// <summary>
    /// Destroy TF3 buffer
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void tf3_buffer_destroy(IntPtr buffer);

    /// <summary>
    /// Set a transform in the buffer
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern bool tf3_set_transform(
        IntPtr buffer,
        ref TF3_Transform transform,
        string authority,
        bool isStatic);

    /// <summary>
    /// Lookup transform between two frames
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern bool tf3_lookup_transform(
        IntPtr buffer,
        string targetFrame,
        string sourceFrame,
        long timeSec,
        long timeNsec,
        ref TF3_Transform transform,
        ref TF3_ErrorCode errorCode);

    /// <summary>
    /// Check if transform can be computed
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern bool tf3_can_transform(
        IntPtr buffer,
        string targetFrame,
        string sourceFrame,
        long timeSec,
        long timeNsec,
        IntPtr errorMsg,
        int errorMsgLen);

    /// <summary>
    /// Clear all transforms
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void tf3_clear(IntPtr buffer);

    /// <summary>
    /// Get current time
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern void tf3_get_current_time(ref long sec, ref long nsec);

    /// <summary>
    /// Get version string
    /// </summary>
    [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    public static extern IntPtr tf3_get_version();

    // [DllImport(LibraryPath, CallingConvention = CallingConvention.Cdecl)]
    // public static extern int tf3_get_all_frame_names(
    //     TF3_BufferCore buffer,
    //     IntPtr frames,
    //     int frames_len);
}