using System.Runtime.InteropServices;
using RobotNet10.RobotApp.Navigation;

namespace RobotNet10.RobotApp.Xloc;

/// <summary>
/// C-compatible structs for xloc C API interop
/// Based on /home/robotics/sonvh/xloc_Linux_0.1.0/_CPack_Packages/Linux/DEB/xloc-0.1.0-Linux/usr/include/xloc/xloc_c_api.h
/// </summary>

/// <summary>
/// Unix timestamp with seconds and nanoseconds (xloc_unix_time_t)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct xloc_unix_time_t
{
    public uint sec;
    public uint nsec;
}

/// <summary>
/// Message header (xloc_header_t)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct xloc_header_t
{
    public uint seq;
    public xloc_unix_time_t stamp;
    public IntPtr frame_id; // char* - must be allocated and freed
}

/// <summary>
/// Odometry message (xloc_odometry_t)
/// CRITICAL: Must match exact C API definition in xloc_c_api.h (lines 138-149)
/// Uses flattened arrays, NOT nested xloc_pose_t or xloc_twist_t structs
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct xloc_odometry_t
{
    public xloc_header_t header;
    public IntPtr child_frame_id; // char* - must be allocated and freed
    
    // Pose with covariance (flattened, not using xloc_pose_t)
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
    public double[] pose_position;      // [3] - x, y, z
    
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
    public double[] pose_orientation;   // [4] - x, y, z, w (quaternion)
    
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 36)]
    public double[] pose_covariance;    // [36] - 6x6 matrix
    
    // Twist with covariance (flattened, not using xloc_twist_t)
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
    public double[] twist_linear;       // [3] - x, y, z
    
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
    public double[] twist_angular;      // [3] - x, y, z
    
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 36)]
    public double[] twist_covariance;   // [36] - 6x6 matrix
}

/// <summary>
/// IMU message (xloc_imu_t)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct xloc_imu_t
{
    public xloc_header_t header;
    
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
    public double[] orientation;                    // [4] - x, y, z, w (quaternion)
    
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)]
    public double[] orientation_covariance;          // [9] - 3x3 matrix
    
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
    public double[] angular_velocity;                // [3] - x, y, z
    
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)]
    public double[] angular_velocity_covariance;      // [9] - 3x3 matrix
    
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
    public double[] linear_acceleration;             // [3] - x, y, z
    
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 9)]
    public double[] linear_acceleration_covariance;   // [9] - 3x3 matrix
}

/// <summary>
/// LaserScan message (xloc_laserscan_t)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct xloc_laserscan_t
{
    public xloc_header_t header;
    public float angle_min;
    public float angle_max;
    public float angle_increment;
    public float time_increment;
    public float scan_time;
    public float range_min;
    public float range_max;
    
    public IntPtr ranges;               // float* - pointer to array
    public nuint ranges_length;          // size_t - number of elements
    
    public IntPtr intensities;          // float* - pointer to array
    public nuint intensities_length;     // size_t - number of elements
}

/// <summary>
/// Status response from xloc (xloc_status_response_t)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct xloc_status_response_t
{
    public byte code;
    public IntPtr message; // char* - must be freed with xloc_free_cstring
}

/// <summary>
/// Pose (xloc_pose_t)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct xloc_pose_t
{
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
    public double[] position;    // [3] - x, y, z
    
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 4)]
    public double[] orientation; // [4] - x, y, z, w (quaternion)
}

/// <summary>
/// Diagnostics information from xloc (xloc_diagnostics_t)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct xloc_diagnostics_t
{
    public xloc_header_t header;
    public byte xloc_state;  // 0=MAPPING, 1=LOCALIZATION, 2=PROCESSING, 3=READY, 4=ERROR
    public IntPtr current_active_map;  // char* - must be freed
    public double reliability;  // 0.0 to 1.0
    public double matching_score;  // SLAM matching quality
}

/// <summary>
/// Occupancy grid map (xloc_occupancy_grid_t)
/// </summary>
[StructLayout(LayoutKind.Sequential)]
public struct xloc_occupancy_grid_t
{
    public xloc_header_t header;
    public float resolution;  // meters per cell
    public uint width;  // cells
    public uint height;  // cells
    public xloc_pose_t origin;  // pose of cell (0,0) in map frame
    public IntPtr data;  // int8_t* - pointer to occupancy data (width*height bytes)
    public nuint data_length;  // size_t - number of bytes
}