using System.Runtime.InteropServices;
using RobotNet10.RobotApp.Xloc;
using RobotNet10.Shared;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Sensor;

namespace RobotNet10.RobotApp.Navigation;

/// <summary>
/// Extension methods to convert C# data structures to Navigation C API handles
/// </summary>
public static class NavigationConversionExtensions
{
    /// <summary>
    /// Convert xloc_occupancy_grid_t to OccupancyGrid
    /// NOTE: Caller must free allocated memory using FreeNavigationOccupancyGrid
    /// </summary>
    public static OccupancyGrid ToNavigationOccupancyGrid(this xloc_occupancy_grid_t xlocGrid)
    {
        var navGrid = new OccupancyGrid
        {   
            info = new MapMetaData
            {
                map_load_time = NavigationNativeInterface.time_create(),
                resolution = xlocGrid.resolution,
                width = xlocGrid.width,
                height = xlocGrid.height,
                origin = new Pose
                {
                    position = new Point
                    {
                        x = xlocGrid.origin.position[0],
                        y = xlocGrid.origin.position[1],
                        z = xlocGrid.origin.position[2]
                    },
                    orientation = new Quaternion
                    {
                        x = xlocGrid.origin.orientation[0],
                        y = xlocGrid.origin.orientation[1],
                        z = xlocGrid.origin.orientation[2],
                        w = xlocGrid.origin.orientation[3]
                    }
                }
            },
            data = IntPtr.Zero
        };

        // Marshal frame_id string
        if (xlocGrid.header.frame_id != IntPtr.Zero)
        {
            navGrid.header.frame_id = Marshal.StringToHGlobalAnsi(
                Marshal.PtrToStringAnsi(xlocGrid.header.frame_id) ?? string.Empty);
        }

        // Copy occupancy data
        if (xlocGrid.data != IntPtr.Zero && xlocGrid.data_length > 0)
        {
            navGrid.data = Marshal.AllocHGlobal((int)xlocGrid.data_length);
            navGrid.data_count = new UIntPtr(xlocGrid.data_length);
            byte[] data = new byte[xlocGrid.data_length];
            Marshal.Copy(xlocGrid.data, data, 0, (int)xlocGrid.data_length);
            Marshal.Copy(data, 0, navGrid.data, (int)xlocGrid.data_length);
        }
        else
        {
            // Ensure data_count is set to 0 when no data
            navGrid.data_count = UIntPtr.Zero;
        }

        return navGrid;
    }

    /// <summary>
    /// Convert Shared.Sensor.LaserScan to NavigationInterop.LaserScan
    /// NOTE: Caller must free allocated memory using FreeNavigationLaserScan
    /// </summary>
    public static LaserScan ToNavigationLaserScan(this global::RobotNet10.Shared.Sensor.LaserScan scan)
    {
        // Create header manually with Marshal-allocated frame_id so we can safely free it after dispatch
        var timeSpan = scan.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch;
        Header scanHeader = new Header
        {
            seq = scan.Header.Seq,
            sec = (uint)timeSpan.TotalSeconds,
            nsec = (uint)((timeSpan.Ticks % TimeSpan.TicksPerSecond) * 100),
            frame_id = Marshal.StringToHGlobalAnsi(scan.Header.FrameId ?? string.Empty)
        };
        
        var navScan = new LaserScan
        {
            header = scanHeader,
            angle_min = (float)scan.AngleMin,
            angle_max = (float)scan.AngleMax,
            angle_increment = (float)scan.AngleIncrement,
            time_increment = (float)scan.TimeIncrement,
            scan_time = (float)scan.ScanTime,
            range_min = (float)scan.RangeMin,
            range_max = (float)scan.RangeMax,
            ranges = IntPtr.Zero,
            ranges_count = 0,
            intensities = IntPtr.Zero,
            intensities_count = 0,
        };

        // Allocate and copy ranges array
        if (scan.Ranges != null && scan.Ranges.Length > 0)
        {
            float[] rangesFloat = Array.ConvertAll(scan.Ranges, value => (float)value);
            int rangesSize = rangesFloat.Length * sizeof(float);
            navScan.ranges = Marshal.AllocHGlobal(rangesSize);
            Marshal.Copy(rangesFloat, 0, navScan.ranges, rangesFloat.Length);
            navScan.ranges_count = (nuint)rangesFloat.Length;
        }

        // Allocate and copy intensities array
        if (scan.Intensities != null && scan.Intensities.Length > 0)
        {
            float[] intensitiesFloat = Array.ConvertAll(scan.Intensities, value => (float)value);
            int intensitiesSize = intensitiesFloat.Length * sizeof(float);
            navScan.intensities = Marshal.AllocHGlobal(intensitiesSize);
            Marshal.Copy(intensitiesFloat, 0, navScan.intensities, intensitiesFloat.Length);
            navScan.intensities_count = (nuint)intensitiesFloat.Length;
        }

        return navScan;
    }

    /// <summary>
    /// Convert Odometry to Odometry
    /// NOTE: Caller must free allocated memory using FreeNavigationOdometry
    /// </summary>
    public static Odometry ToNavigationOdometry(this global::RobotNet10.Shared.Sensor.Odometry odom)
    {
        // Create header using header_create (like in Program.cs)
        Header odomHeader = NavigationNativeInterface.header_create(odom.Header.FrameId ?? string.Empty);
        
        // Update header with sequence and timestamp
        odomHeader.seq = odom.Header.Seq;
        odomHeader.sec = (uint)(odom.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch).TotalSeconds;
        odomHeader.nsec = (uint)(((odom.Header.Stamp.ToUniversalTime() - DateTime.UnixEpoch).Ticks % TimeSpan.TicksPerSecond) * 100);
        
        var navOdom = new Odometry
        {
            header = odomHeader,
            child_frame_id = IntPtr.Zero,
            pose = new PoseWithCovariance
            {
                pose = new Pose
                {
                    position = new Point
                    {
                        x = odom.Pose.Pose.Position.X,
                        y = odom.Pose.Pose.Position.Y,
                        z = odom.Pose.Pose.Position.Z
                    },
                    orientation = new Quaternion
                    {
                        x = odom.Pose.Pose.Orientation.X,
                        y = odom.Pose.Pose.Orientation.Y,
                        z = odom.Pose.Pose.Orientation.Z,
                        w = odom.Pose.Pose.Orientation.W
                    }
                },
                covariance = IntPtr.Zero,
                covariance_count = UIntPtr.Zero
            },
            twist = new TwistWithCovariance
            {
                twist = new Twist
                {
                    linear = new Vector3
                    {
                        x = odom.Twist.Twist.Linear.X,
                        y = odom.Twist.Twist.Linear.Y,
                        z = odom.Twist.Twist.Linear.Z
                    },
                    angular = new Vector3
                    {
                        x = odom.Twist.Twist.Angular.X,
                        y = odom.Twist.Twist.Angular.Y,
                        z = odom.Twist.Twist.Angular.Z
                    }
                },
                covariance = IntPtr.Zero,
                covariance_count = UIntPtr.Zero
            }
        };

        // Marshal child_frame_id string
        if (!string.IsNullOrEmpty(odom.ChildFrameId))
        {
            navOdom.child_frame_id = Marshal.StringToHGlobalAnsi(odom.ChildFrameId);
        }

        // Allocate and copy pose covariance
        if (odom.Pose.Covariance != null && odom.Pose.Covariance.Length == 36)
        {
            int covarianceSize = 36 * sizeof(double);
            navOdom.pose.covariance = Marshal.AllocHGlobal(covarianceSize);
            Marshal.Copy(odom.Pose.Covariance, 0, navOdom.pose.covariance, 36);
            navOdom.pose.covariance_count = new UIntPtr(36);
        }

        // Allocate and copy twist covariance
        if (odom.Twist.Covariance != null && odom.Twist.Covariance.Length == 36)
        {
            int covarianceSize = 36 * sizeof(double);
            navOdom.twist.covariance = Marshal.AllocHGlobal(covarianceSize);
            Marshal.Copy(odom.Twist.Covariance, 0, navOdom.twist.covariance, 36);
            navOdom.twist.covariance_count = new UIntPtr(36);
        }

        return navOdom;
    }

    /// <summary>
    /// Allocate unmanaged memory for OccupancyGrid and return IntPtr
    /// NOTE: Caller must free using FreeNavigationOccupancyGridPtr
    /// </summary>
    public static IntPtr AllocateNavigationOccupancyGrid(OccupancyGrid grid)
    {
        int size = Marshal.SizeOf<OccupancyGrid>();
        IntPtr ptr = Marshal.AllocHGlobal(size);
        Marshal.StructureToPtr(grid, ptr, false);
        return ptr;
    }

    /// <summary>
    /// Allocate unmanaged memory for LaserScanHandle and return IntPtr
    /// NOTE: Caller must free using FreeNavigationLaserScanPtr
    /// </summary>
    public static IntPtr AllocateNavigationLaserScan(LaserScan scan)
    {
        int size = Marshal.SizeOf<LaserScan>();
        IntPtr ptr = Marshal.AllocHGlobal(size);
        Marshal.StructureToPtr(scan, ptr, false);
        return ptr;
    }

    /// <summary>
    /// Allocate unmanaged memory for Odometry and return IntPtr
    /// NOTE: Caller must free using FreeNavigationOdometryPtr
    /// </summary>
    public static IntPtr AllocateNavigationOdometry(Odometry odom)
    {
        int size = Marshal.SizeOf<Odometry>();
        IntPtr ptr = Marshal.AllocHGlobal(size);
        Marshal.StructureToPtr(odom, ptr, false);
        return ptr;
    }

    #region Memory Management

    /// <summary>
    /// Free memory allocated for OccupancyGrid (both struct and IntPtr)
    /// </summary>
    public static void FreeNavigationOccupancyGridPtr(IntPtr ptr)
    {
        if (ptr == IntPtr.Zero)
            return;

        var grid = Marshal.PtrToStructure<OccupancyGrid>(ptr);
        FreeNavigationOccupancyGrid(ref grid);
        Marshal.FreeHGlobal(ptr);
    }

    /// <summary>
    /// Free memory allocated for OccupancyGrid
    /// </summary>
    public static void FreeNavigationOccupancyGrid(ref OccupancyGrid grid)
    {
        if (grid.header.frame_id != IntPtr.Zero)
        {
            Marshal.FreeHGlobal(grid.header.frame_id);
            grid.header.frame_id = IntPtr.Zero;
        }

        if (grid.data != IntPtr.Zero)
        {
            Marshal.FreeHGlobal(grid.data);
            grid.data = IntPtr.Zero;
        }
    }

    /// <summary>
    /// Free memory allocated for LaserScanHandle (both struct and IntPtr)
    /// </summary>
    public static void FreeNavigationLaserScanPtr(IntPtr ptr)
    {
        if (ptr == IntPtr.Zero)
            return;

        var scan = Marshal.PtrToStructure<LaserScan>(ptr);
        FreeNavigationLaserScan(ref scan);
        Marshal.FreeHGlobal(ptr);
    }

    /// <summary>
    /// Free memory allocated for LaserScanHandle
    /// </summary>
    public static void FreeNavigationLaserScan(ref LaserScan scan)
    {
        // frame_id is allocated by Marshal.StringToHGlobalAnsi(), so use Marshal.FreeHGlobal()
        if (scan.header.frame_id != IntPtr.Zero)
        {
            Marshal.FreeHGlobal(scan.header.frame_id);
            scan.header.frame_id = IntPtr.Zero;
        }

        // ranges and intensities are allocated by Marshal.AllocHGlobal(), so use Marshal.FreeHGlobal()
        if (scan.ranges != IntPtr.Zero)
        {
            Marshal.FreeHGlobal(scan.ranges);
            scan.ranges = IntPtr.Zero;
        }

        if (scan.intensities != IntPtr.Zero)
        {
            Marshal.FreeHGlobal(scan.intensities);
            scan.intensities = IntPtr.Zero;
        }
    }

    /// <summary>
    /// Free memory allocated for Odometry (both struct and IntPtr)
    /// </summary>
    public static void FreeNavigationOdometryPtr(IntPtr ptr)
    {
        if (ptr == IntPtr.Zero)
            return;

        var odom = Marshal.PtrToStructure<Odometry>(ptr);
        FreeNavigationOdometry(ref odom);
        Marshal.FreeHGlobal(ptr);
    }

    /// <summary>
    /// Free memory allocated for Odometry
    /// </summary>
    public static void FreeNavigationOdometry(ref Odometry odom)
    {
        // header.frame_id is allocated by header_create() using strdup() (malloc), so use nav_c_api_free_string()
        if (odom.header.frame_id != IntPtr.Zero)
        {
            NavigationNativeInterface.nav_c_api_free_string(odom.header.frame_id);
            odom.header.frame_id = IntPtr.Zero;
        }

        // child_frame_id is allocated by Marshal.StringToHGlobalAnsi(), so use Marshal.FreeHGlobal()
        if (odom.child_frame_id != IntPtr.Zero)
        {
            Marshal.FreeHGlobal(odom.child_frame_id);
            odom.child_frame_id = IntPtr.Zero;
        }

        // covariance arrays are allocated by Marshal.AllocHGlobal(), so use Marshal.FreeHGlobal()
        if (odom.pose.covariance != IntPtr.Zero)
        {
            Marshal.FreeHGlobal(odom.pose.covariance);
            odom.pose.covariance = IntPtr.Zero;
            odom.pose.covariance_count = UIntPtr.Zero;
        }

        if (odom.twist.covariance != IntPtr.Zero)
        {
            Marshal.FreeHGlobal(odom.twist.covariance);
            odom.twist.covariance = IntPtr.Zero;
            odom.twist.covariance_count = UIntPtr.Zero;
        }
    }

    #endregion
}