using System.Runtime.InteropServices;
using RobotNet10.Shared;
using RobotNet10.Shared.Geometry;
using RobotNet10.Shared.Sensor;
// using RobotNet10.Shared.Numbers;

namespace RobotNet10.RobotApp.Xloc;

/// <summary>
/// Extension methods to convert C# sensor structs to xloc C-compatible structs
/// </summary>
public static class XlocConversionExtensions
{
    /// <summary>
    /// Convert C# Header to xloc_header_t
    /// NOTE: Caller must free FrameId using Marshal.FreeHGlobal
    /// </summary>
    public static xloc_header_t ToXlocHeader(this Header header)
    {
        var xlocHeader = new xloc_header_t
        {
            seq = header.Seq,
            stamp = header.Stamp.ToXlocUnixTime(),
            frame_id = IntPtr.Zero
        };

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

        return xlocHeader;
    }

    /// <summary>
    /// Convert DateTime to xloc_unix_time_t (Unix epoch time)
    /// </summary>
    // public static xloc_unix_time_t ToXlocUnixTime(this DateTime timestamp)
    // {
    //     // Convert to Unix time (seconds and nanoseconds since 1970-01-01)
    //     var epoch = new DateTime(1970, 1, 1, 0, 0, 0, DateTimeKind.Utc);
    //     var duration = timestamp.ToUniversalTime() - epoch;
        
    //     var totalSeconds = (long)duration.TotalSeconds;
    //     var nanoseconds = (long)((duration.TotalSeconds - totalSeconds) * 1_000_000_000);
        
    //     var xlocTime = new xloc_unix_time_t
    //     {
    //         sec = (uint)totalSeconds,
    //         nsec = (uint)nanoseconds
    //     };
    //     return xlocTime;
    // }
    public static xloc_unix_time_t ToXlocUnixTime(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 xloc_unix_time_t
    {
        sec = sec,
        nsec = nsec
    };
}


    /// <summary>
    /// Convert C# Odometry to xloc_odometry_t
    /// NOTE: Caller must free frame_id and child_frame_id using FreeXlocOdometry
    /// </summary>
    public static xloc_odometry_t ToXlocOdometry(this Odometry odom)
    {
        var xlocOdom = new xloc_odometry_t
        {
            header = odom.Header.ToXlocHeader(),
            child_frame_id = Marshal.StringToHGlobalAnsi(odom.ChildFrameId ?? string.Empty),
            
            // Pose - flattened arrays (matching C API)
            pose_position = new double[]
            {
                odom.Pose.Pose.Position.X,
                odom.Pose.Pose.Position.Y,
                odom.Pose.Pose.Position.Z
            },
            pose_orientation = new double[]
            {
                odom.Pose.Pose.Orientation.X,
                odom.Pose.Pose.Orientation.Y,
                odom.Pose.Pose.Orientation.Z,
                odom.Pose.Pose.Orientation.W
            },
            pose_covariance = odom.Pose.Covariance ?? new double[36],
            
            // Twist - flattened arrays (matching C API)
            twist_linear = new double[]
            {
                odom.Twist.Twist.Linear.X,
                odom.Twist.Twist.Linear.Y,
                odom.Twist.Twist.Linear.Z
            },
            twist_angular = new double[]
            {
                odom.Twist.Twist.Angular.X,
                odom.Twist.Twist.Angular.Y,
                odom.Twist.Twist.Angular.Z
            },
            twist_covariance = odom.Twist.Covariance ?? new double[36]
        };

        return xlocOdom;
    }

    /// <summary>
    /// Convert C# Imu to xloc_imu_t
    /// NOTE: Caller must free FrameId using FreeXlocImu
    /// </summary>
    public static xloc_imu_t ToXlocImu(this Imu imu)
    {
        // Validate and normalize quaternion
        var quat = imu.Orientation;
        double quatLength = Math.Sqrt(quat.X * quat.X + quat.Y * quat.Y + quat.Z * quat.Z + quat.W * quat.W);
        
        if (quatLength < 0.001 || double.IsNaN(quatLength) || double.IsInfinity(quatLength))
        {
            // Invalid quaternion, use identity
            Console.WriteLine("[XLOC] Invalid IMU quaternion detected, using identity");
            quat = new RobotNet10.Shared.Geometry.Quaternion { X = 0, Y = 0, Z = 0, W = 1 };
            quatLength = 1.0;
        }
        
        // Normalize quaternion
        quat = new RobotNet10.Shared.Geometry.Quaternion 
        { 
            X = quat.X / quatLength,
            Y = quat.Y / quatLength,
            Z = quat.Z / quatLength,
            W = quat.W / quatLength
        };
        
        // Validate angular velocity (clamp extremely large values)
        const double MAX_ANGULAR_VEL = 10.0; // rad/s
        var angVel = imu.AngularVelocity;
        if (Math.Abs(angVel.X) > MAX_ANGULAR_VEL || Math.Abs(angVel.Y) > MAX_ANGULAR_VEL || 
            Math.Abs(angVel.Z) > MAX_ANGULAR_VEL ||
            double.IsNaN(angVel.X) || double.IsNaN(angVel.Y) || double.IsNaN(angVel.Z))
        {
            Console.WriteLine($"[XLOC] Invalid angular velocity detected: ({angVel.X}, {angVel.Y}, {angVel.Z}), clamping");
            angVel = new Vector3 
            { 
                X = Math.Clamp(double.IsNaN(angVel.X) ? 0 : angVel.X, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL),
                Y = Math.Clamp(double.IsNaN(angVel.Y) ? 0 : angVel.Y, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL),
                Z = Math.Clamp(double.IsNaN(angVel.Z) ? 0 : angVel.Z, -MAX_ANGULAR_VEL, MAX_ANGULAR_VEL)
            };
        }
        
        var xlocImu = new xloc_imu_t
        {
            header = imu.Header.ToXlocHeader(),
            
            orientation = new double[4]
            {
                quat.X,
                quat.Y,
                quat.Z,
                quat.W
            },
            orientation_covariance = imu.OrientationCovariance ?? new double[9],
            
            angular_velocity = new double[3]
            {
                angVel.X,
                angVel.Y,
                angVel.Z
            },
            angular_velocity_covariance = imu.AngularVelocityCovariance ?? new double[9],
            
            linear_acceleration = new double[3]
            {
                imu.LinearAcceleration.X,
                imu.LinearAcceleration.Y,
                imu.LinearAcceleration.Z
            },
            linear_acceleration_covariance = imu.LinearAccelerationCovariance ?? new double[9]
        };

        return xlocImu;
    }

    /// <summary>
    /// Convert C# LaserScan to xloc_laserscan_t
    /// NOTE: Caller must free all allocated memory using FreeXlocLaserScan
    /// </summary>
    public static xloc_laserscan_t ToXlocLaserScan(this LaserScan scan)
    {
        var xlocScan = new xloc_laserscan_t
        {
            header = scan.Header.ToXlocHeader(),
            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 = 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($"[XLOC] Sanitized {invalidCount}/{scan.Ranges.Length} invalid laser scan ranges");
            }
            
            int rangesSize = sanitizedRanges.Length * sizeof(float);
            xlocScan.ranges = Marshal.AllocHGlobal(rangesSize);
            Marshal.Copy(sanitizedRanges, 0, xlocScan.ranges, sanitizedRanges.Length);
        }
        else
        {
            xlocScan.ranges = IntPtr.Zero;
        }

        // Intensities are optional; when provided they should match ranges count.
        if (scan.Intensities != null && scan.Intensities.Length > 0)
        {
            int expectedLength = scan.Ranges?.Length ?? 0;
            if (scan.Intensities.Length == expectedLength)
            {
                var sanitizedIntensities = new float[scan.Intensities.Length];
                for (int i = 0; i < scan.Intensities.Length; i++)
                {
                    float intensity = (float)scan.Intensities[i];
                    sanitizedIntensities[i] = (float.IsNaN(intensity) || float.IsInfinity(intensity)) ? 0f : intensity;
                }

                int intensitiesSize = sanitizedIntensities.Length * sizeof(float);
                xlocScan.intensities = Marshal.AllocHGlobal(intensitiesSize);
                Marshal.Copy(sanitizedIntensities, 0, xlocScan.intensities, sanitizedIntensities.Length);
                xlocScan.intensities_length = (nuint)sanitizedIntensities.Length;
            }
            else
            {
                Console.WriteLine($"[XLOC] Ignoring intensities due to length mismatch: ranges={expectedLength}, intensities={scan.Intensities.Length}");
                xlocScan.intensities = IntPtr.Zero;
                xlocScan.intensities_length = 0;
            }
        }
        else
        {
            xlocScan.intensities = IntPtr.Zero;
            xlocScan.intensities_length = 0;
        }

        return xlocScan;
    }

    #region Memory Management

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

        if (odom.child_frame_id != IntPtr.Zero)
        {
            Marshal.FreeHGlobal(odom.child_frame_id);
            odom.child_frame_id = IntPtr.Zero;
        }
    }

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

    /// <summary>
    /// Free memory allocated for xloc_laserscan_t
    /// </summary>
    public static void FreeXlocLaserScan(ref xloc_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>
    /// Get managed string from xloc status response and free the C string
    /// </summary>
    public static string GetMessageAndFree(ref xloc_status_response_t response)
    {
        if (response.message == IntPtr.Zero)
            return string.Empty;

        string message = Marshal.PtrToStringUTF8(response.message) ?? string.Empty;
        XlocNativeInterface.xloc_free_cstring(response.message);
        response.message = IntPtr.Zero;
        return message;
    }

    #endregion
}