using System;
using System.Runtime.InteropServices;

namespace NavigationExample
{
    // ============================================================================
    // Structures
    // ============================================================================
    [StructLayout(LayoutKind.Sequential)]
    public struct Point
    {
        public double x;
        public double y;
        public double z;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Pose2D
    {
        public double x;
        public double y;
        public double theta;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Twist2D
    {
        public double x;
        public double y;
        public double theta;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Quaternion
    {
        public double x;
        public double y;
        public double z;
        public double w;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Position
    {
        public double x;
        public double y;
        public double z;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Pose
    {
        public Point position;
        public Quaternion orientation;
    }


    [StructLayout(LayoutKind.Sequential)]
    public struct Vector3
    {
        public double x;
        public double y;
        public double z;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Twist
    {
        public Vector3 linear;
        public Vector3 angular;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Header
    {
        public uint seq;
        public uint sec;
        public uint nsec;
        public IntPtr frame_id; // char*
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct PoseStamped
    {
        public Header header;
        public Pose pose;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Twist2DStamped
    {
        public Header header;
        public Twist2D velocity;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct LaserScan
    {
        public Header 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;
        public UIntPtr ranges_count;
        public IntPtr intensities;
        public UIntPtr intensities_count;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct PointCloud
    {
        public Header header;
        public IntPtr points;
        public UIntPtr points_count;
        public IntPtr channels;
        public UIntPtr channels_count;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct PointCloud2
    {
        public Header header;
        public uint height;
        public uint width;
        public IntPtr fields;
        public UIntPtr fields_count;
        [MarshalAs(UnmanagedType.I1)]
        public bool is_bigendian;
        public uint point_step;
        public uint row_step;
        public IntPtr data;
        public UIntPtr data_count;
        [MarshalAs(UnmanagedType.I1)]
        public bool is_dense;
    } 

    [StructLayout(LayoutKind.Sequential)]
    public struct PoseWithCovariance
    {
        public Pose pose;
        public IntPtr covariance;
        public UIntPtr covariance_count;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct TwistWithCovariance    {
        public Twist twist;
        public IntPtr covariance;
        public UIntPtr covariance_count;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Odometry
    {
        public Header header;
        public IntPtr child_frame_id;
        public PoseWithCovariance pose;
        public TwistWithCovariance twist;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct OccupancyGrid
    {
        public Header header;
        public MapMetaData info;
        public IntPtr data;
        public UIntPtr data_count;
    }


    [StructLayout(LayoutKind.Sequential)]
    public struct MapMetaData
    {
        public Time map_load_time;
        public float resolution;
        public uint width;
        public uint height;
        public Pose origin;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Time
    {
        public uint sec;
        public uint nsec;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Point32
    {
        public float x;
        public float y;
        public float z;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Pose2DStamped
    {
        public Header header;
        public Pose2D pose;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Polygon
    {
        public IntPtr points;
        public UIntPtr points_count;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct PolygonStamped
    {
        public Header header;
        public Polygon polygon;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct OccupancyGridUpdate
    {
        public Header header;
        public int x;
        public int y;
        public uint width;
        public uint height;
        public IntPtr data;
        public UIntPtr data_count;
    }

    [StructLayout(LayoutKind.Sequential)]
    public struct Path2D
    {
        public Header header;
        public IntPtr poses;
        public UIntPtr poses_count;
    }

    /// <summary>Name (char*) + OccupancyGrid for static map list API.</summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct NamedOccupancyGrid
    {
        public IntPtr name;
        public OccupancyGrid grid;
    }

    /// <summary>Name (char*) + LaserScan for laser scan list API.</summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct NamedLaserScan
    {
        public IntPtr name;
        public LaserScan scan;
    }

    /// <summary>Name (char*) + PointCloud for point cloud list API.</summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct NamedPointCloud
    {
        public IntPtr name;
        public PointCloud cloud;
    }

    /// <summary>Name (char*) + PointCloud2 for point cloud2 list API.</summary>
    [StructLayout(LayoutKind.Sequential)]
    public struct NamedPointCloud2
    {
        public IntPtr name;
        public PointCloud2 cloud;
    }


}