using System;
using System.Runtime.InteropServices;

namespace NavigationExample
{

   // ============================================================================
    // TF3 C API - P/Invoke wrapper for libtf3 (tf3 BufferCore)
    // ============================================================================
    public static class TF3API
    {
        private const string Tf3DllName = "/usr/local/lib/libtf3.so"; // Linux; Windows: tf3.dll, macOS: libtf3.dylib

        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
        }

        [StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi)]
        public struct TF3_Transform
        {
            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;
            public double translation_x;
            public double translation_y;
            public double translation_z;
            public double rotation_x;
            public double rotation_y;
            public double rotation_z;
            public double rotation_w;
        }

        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
        public static extern IntPtr tf3_buffer_create(int cache_time_sec);

        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
        public static extern void tf3_buffer_destroy(IntPtr buffer);

        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool tf3_set_transform(
            IntPtr buffer,
            ref TF3_Transform transform,
            string authority,
            [MarshalAs(UnmanagedType.I1)] bool is_static);

        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool tf3_lookup_transform(
            IntPtr buffer,
            string target_frame,
            string source_frame,
            long time_sec,
            long time_nsec,
            out TF3_Transform transform,
            out TF3_ErrorCode error_code);

        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool tf3_lookup_transform_full(
            IntPtr buffer,
            string target_frame,
            long target_time_sec,
            long target_time_nsec,
            string source_frame,
            long source_time_sec,
            long source_time_nsec,
            string fixed_frame,
            out TF3_Transform transform,
            out TF3_ErrorCode error_code);

        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl, CharSet = CharSet.Ansi)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool tf3_can_transform(
            IntPtr buffer,
            string target_frame,
            string source_frame,
            long time_sec,
            long time_nsec,
            System.Text.StringBuilder error_msg,
            int error_msg_len);

        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
        public static extern int tf3_get_all_frame_names(IntPtr buffer, System.Text.StringBuilder frames, int frames_len);

        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool tf3_get_frame_tree(IntPtr buffer, System.Text.StringBuilder output, int output_len);

        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
        public static extern void tf3_clear(IntPtr buffer);

        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
        public static extern void tf3_get_current_time(out long sec, out long nsec);

        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
        [return: MarshalAs(UnmanagedType.I1)]
        public static extern bool tf3_get_last_error(IntPtr buffer, System.Text.StringBuilder error_msg, int error_msg_len);

        [DllImport(Tf3DllName, CallingConvention = CallingConvention.Cdecl)]
        public static extern IntPtr tf3_get_version();

        /// <summary>Helper: create TF3_Transform for static transform (identity or given pose).</summary>
        public static TF3_Transform CreateStaticTransform(string parentFrame, string childFrame,
            double tx = 0, double ty = 0, double tz = 0,
            double qx = 0, double qy = 0, double qz = 0, double qw = 1)
        {
            var t = new TF3_Transform();
            t.timestamp_sec = 0;
            t.timestamp_nsec = 0;
            t.frame_id = parentFrame ?? "";
            t.child_frame_id = childFrame ?? "";
            t.translation_x = tx;
            t.translation_y = ty;
            t.translation_z = tz;
            t.rotation_x = qx;
            t.rotation_y = qy;
            t.rotation_z = qz;
            t.rotation_w = qw;
            return t;
        }
    }
}