using System.Runtime.InteropServices;
using RobotNet10.Shared;
using RobotNet10.Shared.Sensor;

namespace RobotNet10.RobotApp.MarkerDetection;

/// <summary>
/// Extension methods to convert C# sensor structs to marker_detection C-compatible structs
/// </summary>
public static class MarkerDetectionConversionExtensions
{
    /// <summary>
    /// Convert C# Header to md_header_t
    /// NOTE: Caller must free FrameId using Marshal.FreeHGlobal
    /// </summary>
    public static md_header_t ToMarkerDetectionHeader(this Header header)
    {
        var mdHeader = new md_header_t
        {
            seq = header.Seq,
            stamp = header.Stamp.ToMarkerDetectionUnixTime(),
            frame_id = IntPtr.Zero
        };

        // Marshal frame_id string to unmanaged memory
        if (!string.IsNullOrEmpty(header.FrameId))
        {
            mdHeader.frame_id = Marshal.StringToHGlobalAnsi(header.FrameId);
        }

        return mdHeader;
    }

    /// <summary>
    /// Convert DateTime to md_unix_time_t (Unix epoch time)
    /// </summary>
    public static md_unix_time_t ToMarkerDetectionUnixTime(this DateTime timestamp)
    {
        var utc = timestamp.Kind == DateTimeKind.Utc
            ? timestamp
            : timestamp.ToUniversalTime();

        long ticksSinceEpoch = utc.Ticks - DateTime.UnixEpoch.Ticks;
        long totalNanoseconds = ticksSinceEpoch * 100; // 1 tick = 100 ns

        uint sec = (uint)(totalNanoseconds / 1_000_000_000);
        uint nsec = (uint)(totalNanoseconds % 1_000_000_000);

        return new md_unix_time_t
        {
            sec = sec,
            nsec = nsec
        };
    }

    /// <summary>
    /// Convert C# LaserScan to md_laserscan_t
    /// NOTE: Caller must free all allocated memory using FreeMarkerDetectionLaserScan
    /// </summary>
    public static md_laserscan_t ToMarkerDetectionLaserScan(this LaserScan scan)
    {
        var mdScan = new md_laserscan_t
        {
            header = scan.Header.ToMarkerDetectionHeader(),
            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_length = (nuint)(scan.Ranges?.Length ?? 0),
            intensities_length = (nuint)(scan.Intensities?.Length ?? 0)
        };

        // Allocate and copy ranges array with validation
        if (scan.Ranges != null && scan.Ranges.Length > 0)
        {
            // Sanitize ranges: replace NaN/Infinity/negative with max range
            var sanitizedRanges = new float[scan.Ranges.Length];
            int invalidCount = 0;
            
            for (int i = 0; i < scan.Ranges.Length; i++)
            {
                float range = (float)scan.Ranges[i];
                if (float.IsNaN(range) || float.IsInfinity(range) || range < 0)
                {
                    sanitizedRanges[i] = (float)scan.RangeMax;
                    invalidCount++;
                }
                else
                {
                    sanitizedRanges[i] = range;
                }
            }
            
            if (invalidCount > 0)
            {
                Console.WriteLine($"[MarkerDetection] Sanitized {invalidCount}/{scan.Ranges.Length} invalid laser scan ranges");
            }
            
            int rangesSize = sanitizedRanges.Length * sizeof(float);
            mdScan.ranges = Marshal.AllocHGlobal(rangesSize);
            Marshal.Copy(sanitizedRanges, 0, mdScan.ranges, sanitizedRanges.Length);
        }
        else
        {
            mdScan.ranges = IntPtr.Zero;
        }

        // Allocate and copy intensities array
        if (scan.Intensities != null && scan.Intensities.Length > 0)
        {
            int intensitiesSize = scan.Intensities.Length * sizeof(float);
            mdScan.intensities = Marshal.AllocHGlobal(intensitiesSize);
            Marshal.Copy(scan.Intensities, 0, mdScan.intensities, scan.Intensities.Length);
        }
        else
        {
            mdScan.intensities = IntPtr.Zero;
        }

        return mdScan;
    }

    #region Memory Management

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

        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 md_rectangle_marker_options_t
    /// </summary>
    public static void FreeMarkerDetectionRectangleOptions(ref md_rectangle_marker_options_t options)
    {
        if (options.marker_frame_id != IntPtr.Zero)
        {
            Marshal.FreeHGlobal(options.marker_frame_id);
            options.marker_frame_id = IntPtr.Zero;
        }
    }

    /// <summary>
    /// Free memory allocated for md_segment_marker_options_t
    /// </summary>
    public static void FreeMarkerDetectionSegmentOptions(ref md_segment_marker_options_t options)
    {
        if (options.marker_frame_id != IntPtr.Zero)
        {
            Marshal.FreeHGlobal(options.marker_frame_id);
            options.marker_frame_id = IntPtr.Zero;
        }
    }

    #endregion
}