BQP/srcs/RobotNet10/RobotApp/Communication/SickScanXdApi/SickScanApiClient.cs

using System.Text;

namespace RobotNet10.RobotApp.SickScanXdApi;

/// <summary>
/// High-level C# wrapper for sick_scan_xd API
/// </summary>
public class SickScanApiClient : IDisposable
{
    private IntPtr _apiHandle;
    private bool _disposed;
    private readonly object _lock = new();
    
    public bool IsInitialized => _apiHandle != IntPtr.Zero;
    
    public SickScanApiClient()
    {
        _apiHandle = IntPtr.Zero;
    }
    
    /// <summary>
    /// Initialize scanner with launchfile-style arguments
    /// </summary>
    public async Task<bool> InitializeAsync(string launchfileArgs, CancellationToken ct = default)
    {
        await Task.Yield(); // Make async
        
        lock (_lock)
        {
            if (_apiHandle != IntPtr.Zero)
                throw new InvalidOperationException("Already initialized");
            
            // Create API handle
            _apiHandle = SickScanApiNative.SickScanApiCreate(0, IntPtr.Zero);
            if (_apiHandle == IntPtr.Zero)
                return false;
            
            // Split launch args by spaces and convert to argc/argv format
            // Format: "scanner_type:=sick_tim_7xx hostname:=192.168.254.11 port:=2112 ..."
            var argsList = new List<string>();
            argsList.Add("sick_scan_xd_api"); // argv[0] = program name
            argsList.AddRange(launchfileArgs.Split(' ', StringSplitOptions.RemoveEmptyEntries));
            
            int argc = argsList.Count;
            IntPtr[] argvPtrs = new IntPtr[argc];
            IntPtr argv = IntPtr.Zero;
            
            try
            {
                // Allocate argv array
                argv = System.Runtime.InteropServices.Marshal.AllocHGlobal(argc * IntPtr.Size);
                
                // Marshal each string argument
                for (int i = 0; i < argc; i++)
                {
                    argvPtrs[i] = System.Runtime.InteropServices.Marshal.StringToHGlobalAnsi(argsList[i]);
                }
                
                // Copy pointers to argv
                System.Runtime.InteropServices.Marshal.Copy(argvPtrs, 0, argv, argc);
                
                // Initialize with CLI args
                var result = SickScanApiNative.SickScanApiInitByCli(_apiHandle, argc, argv);
                
                if (result != SickScanApiNative.SICK_SCAN_API_SUCCESS)
                {
                    SickScanApiNative.SickScanApiRelease(_apiHandle);
                    _apiHandle = IntPtr.Zero;
                    return false;
                }
                
                return true;
            }
            finally
            {
                // Free all allocated memory
                for (int i = 0; i < argc && i < argvPtrs.Length; i++)
                {
                    if (argvPtrs[i] != IntPtr.Zero)
                        System.Runtime.InteropServices.Marshal.FreeHGlobal(argvPtrs[i]);
                }
                if (argv != IntPtr.Zero)
                    System.Runtime.InteropServices.Marshal.FreeHGlobal(argv);
            }
        }
    }
    
    /// <summary>
    /// Poll for next polar point cloud (blocking with timeout)
    /// </summary>
    public SickScanApiNative.SickScanPointCloudMsg? PollPolarPointCloud(double timeoutSec = 1.0)
    {
        if (_apiHandle == IntPtr.Zero)
            throw new InvalidOperationException("Not initialized");
        
        var msg = new SickScanApiNative.SickScanPointCloudMsg
        {
            header = new SickScanApiNative.SickScanHeader
            {
                frame_id = new byte[256]
            },
            fields = new SickScanApiNative.SickScanPointFieldArray(),
            data = new SickScanApiNative.SickScanUint8Array(),
            topic = new byte[256]
        };
        
        var result = SickScanApiNative.SickScanApiWaitNextPolarPointCloudMsg(
            _apiHandle, ref msg, timeoutSec);
        
        if (result == SickScanApiNative.SICK_SCAN_API_SUCCESS)
            return msg;
        
        if (result == SickScanApiNative.SICK_SCAN_API_TIMEOUT)
            return null;
        
        throw new Exception($"Failed to poll point cloud: error code {result}");
    }
    
    /// <summary>
    /// Free point cloud message memory
    /// </summary>
    public void FreePointCloudMsg(ref SickScanApiNative.SickScanPointCloudMsg msg)
    {
        if (_apiHandle == IntPtr.Zero)
            return;
        
        SickScanApiNative.SickScanApiFreePointCloudMsg(_apiHandle, ref msg);
    }
    
    /// <summary>
    /// Close scanner connection
    /// </summary>
    public void Close()
    {
        lock (_lock)
        {
            if (_apiHandle == IntPtr.Zero)
                return;
            
            SickScanApiNative.SickScanApiClose(_apiHandle);
            SickScanApiNative.SickScanApiRelease(_apiHandle);
            _apiHandle = IntPtr.Zero;
        }
    }
    
    public void Dispose()
    {
        if (_disposed)
            return;
        
        Close();
        _disposed = true;
        GC.SuppressFinalize(this);
    }
}