BQP/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Services/SocketCanBus.cs

using Microsoft.Extensions.Logging;
using RobotNet10.CANOpen.Interfaces;
using SocketCANSharp;
using System.Net.Sockets;
using System.Runtime.InteropServices;

namespace RobotNet10.CANOpen.Services;

public class SocketCanBus : ICanBus
{
    private readonly string _interfaceName;
    private readonly ILogger<SocketCanBus>? _logger;
    private SafeFileDescriptorHandle? _socketHandle;
    private bool _isConnected;
    private Task? _receiveTask;
    private CancellationTokenSource? _receiveCts;
    
    public string InterfaceName => _interfaceName;
    public bool IsConnected => _isConnected;
    
    public event EventHandler<CanFrameReceivedEventArgs>? FrameReceived;
    
    public SocketCanBus(string interfaceName, ILogger<SocketCanBus>? logger = null)
    {
        _interfaceName = interfaceName ?? throw new ArgumentNullException(nameof(interfaceName));
        _logger = logger;
    }

    public async Task ConnectAsync(CancellationToken cancellationToken = default)
    {
        if (_isConnected)
            return;
        
        await Task.Run(() =>
        {
            _socketHandle = LibcNativeMethods.Socket(
                SocketCanConstants.PF_CAN, 
                SocketType.Raw, 
                SocketCanProtocolType.CAN_RAW);
            
            if (_socketHandle.IsInvalid)
                throw new InvalidOperationException("Failed to create CAN socket");
            
            var ifr = new Ifreq(_interfaceName);
            int ioctlResult = LibcNativeMethods.Ioctl(_socketHandle, SocketCanConstants.SIOCGIFINDEX, ifr);
            if (ioctlResult == -1)
                throw new InvalidOperationException($"Failed to find interface {_interfaceName}");
            
            var addr = new SockAddrCan(ifr.IfIndex);
            int bindResult = LibcNativeMethods.Bind(_socketHandle, addr, Marshal.SizeOf<SockAddrCan>());
            if (bindResult == -1)
                throw new InvalidOperationException("Failed to bind to CAN interface");
            
            _isConnected = true;
        }, cancellationToken);
        
        _receiveCts = new CancellationTokenSource();
        _receiveTask = Task.Run(() => ReceiveLoop(_receiveCts.Token), _receiveCts.Token);
    }
    
    public async Task DisconnectAsync(CancellationToken cancellationToken = default)
    {
        if (!_isConnected)
            return;
        
        _isConnected = false;
        
        // Dispose the handle wrapper (should be safe even if already closed)
        if (_socketHandle != null)
        {
            try
            {
                // Close socket to release resources
                int closeResult = LibcNativeMethods.Close(_socketHandle.DangerousGetHandle());
                if (closeResult != 0)
                {
                    int errorCode = Marshal.GetLastPInvokeError();
                    _logger?.LogWarning("Close socket returned error code {ErrorCode} for {InterfaceName}", errorCode, _interfaceName);
                }
                _socketHandle.Dispose();
            }
            catch (Exception ex)
            {
                _logger?.LogWarning(ex, "Error disposing socket handle wrapper");
            }
            _socketHandle = null;
        }
        
        // Cancel receive task after closing socket
        if (_receiveCts != null)
        {
            _receiveCts.Cancel();
            
            // Wait for receive task to finish (should exit quickly now that socket is closed)
            // Use timeout to avoid hanging if receive task is stuck in blocking Read()
            if (_receiveTask != null)
            {
                try
                {
                    using (var timeoutCts = new CancellationTokenSource(TimeSpan.FromSeconds(2)))
                    {
                        await _receiveTask.WaitAsync(timeoutCts.Token);
                    }
                }
                catch (OperationCanceledException)
                {
                    _logger?.LogWarning("Timeout waiting for receive task to finish for SocketCanBus {InterfaceName}", _interfaceName);
                }
                catch (Exception ex)
                {
                    _logger?.LogWarning(ex, "Error waiting for receive task to finish");
                }
            }
            
            _receiveCts.Dispose();
            _receiveCts = null;
        }
    }
    
    public Task SendFrameAsync(uint canId, byte[] data, CancellationToken cancellationToken = default)
    {
        try
        {
            if (!_isConnected || _socketHandle == null)
                throw new InvalidOperationException("Not connected to CAN bus");

            if (data.Length > 8)
                throw new ArgumentException("CAN frame data cannot exceed 8 bytes");

            return Task.Run(() =>
            {
                var frame = new CanFrame
                {
                    CanId = canId,
                    Length = (byte)data.Length,
                    Data = new byte[8]
                };

                for (int i = 0; i < data.Length; i++)
                    frame.Data[i] = data[i];

                int frameSize = Marshal.SizeOf<CanFrame>();
                int bytesWritten = LibcNativeMethods.Write(_socketHandle, ref frame, frameSize);

                if (bytesWritten != frameSize)
                    throw new InvalidOperationException($"Failed to send CAN frame with ID 0x{canId:X3} on interface {_interfaceName}. Bytes written: {bytesWritten}, Frame size: {frameSize}");

            }, cancellationToken);
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Failed to send CAN frame with ID 0x{CanId:X3} on interface {InterfaceName}", canId, _interfaceName);
            throw;
        }

    }

    private void ReceiveLoop(CancellationToken cancellationToken)
    {
        if (_socketHandle == null)
            return;
        
        int frameSize = Marshal.SizeOf<CanFrame>();
        
        try
        {
            while (!cancellationToken.IsCancellationRequested && _isConnected)
            {
                // Check if socket handle is still valid before attempting to read
                if (_socketHandle == null || _socketHandle.IsInvalid)
                {
                    break;
                }
                
                try
                {
                    var readFrame = new CanFrame();
                    int nReadBytes = LibcNativeMethods.Read(_socketHandle, ref readFrame, frameSize);
                    
                    // Read returns 0 if socket is closed, negative on error
                    if (nReadBytes <= 0)
                    {
                        // Socket closed or error occurred
                        break;
                    }
                    
                    if (nReadBytes > 0)
                    {
                        // Check again after blocking Read() returns
                        if (cancellationToken.IsCancellationRequested || !_isConnected)
                        {
                            break;
                        }
                        
                        // Check if socket handle is still valid before ioctl
                        if (_socketHandle == null || _socketHandle.IsInvalid)
                        {
                            break;
                        }
                        
                        var timeval = new Timeval();
                        int result = LibcNativeMethods.Ioctl(_socketHandle, SocketCanConstants.SIOCGSTAMP, timeval);
                        
                        DateTime timestamp = result != -1
                            ? DateTimeOffset.FromUnixTimeSeconds(timeval.Seconds)
                                .AddMicroseconds(timeval.Microseconds).DateTime
                            : DateTime.UtcNow;
                        
                        byte[] data = new byte[readFrame.Length];
                        for (int i = 0; i < readFrame.Length; i++)
                            data[i] = readFrame.Data[i];
                        
                        FrameReceived?.Invoke(this, new CanFrameReceivedEventArgs(
                            readFrame.CanId,
                            data,
                            timestamp));
                    }
                }
                catch (Exception ex)
                {
                    // If cancellation is requested, exit gracefully
                    if (cancellationToken.IsCancellationRequested)
                    {
                        break;
                    }
                    
                    // If socket is closed/disposed, exit gracefully
                    if (_socketHandle == null || _socketHandle.IsInvalid || !_isConnected)
                    {
                        break;
                    }
                    
                    // Other exceptions should be logged and re-thrown
                    _logger?.LogError(ex, "Error in receive loop for SocketCanBus {InterfaceName}", _interfaceName);
                    throw;
                }
            }
        }
        catch (Exception ex)
        {
            _logger?.LogError(ex, "Error in receive loop for SocketCanBus {InterfaceName}", _interfaceName);
        }
    }
    
    public void Dispose()
    {
        // Cancel receive task first
        if (_receiveCts != null)
        {
            try
            {
                _receiveCts.Cancel();
            }
            catch (ObjectDisposedException)
            {
                // Already disposed, ignore
            }
        }

        // Wait for receive task to finish (with timeout to avoid hanging)
        if (_receiveTask != null)
        {
            try
            {
                if (!_receiveTask.Wait(TimeSpan.FromSeconds(2)))
                {
                    _logger?.LogWarning("Timeout waiting for receive task to finish during disposal");
                }
            }
            catch (Exception ex)
            {
                _logger?.LogError(ex, "Error waiting for receive task during disposal");
            }
        }

        // Dispose receive CTS
        try
        {
            _receiveCts?.Dispose();
        }
        catch (ObjectDisposedException)
        {
            // Already disposed, ignore
        }
        _receiveCts = null;
        _receiveTask = null;

        // Disconnect and dispose socket handle
        _isConnected = false;
        
        // Close socket directly if still valid
        if (_socketHandle != null)
        {
            try
            {
                // Close socket to release resources
                int closeResult = LibcNativeMethods.Close(_socketHandle.DangerousGetHandle());
                if (closeResult != 0)
                {
                    int errorCode = Marshal.GetLastPInvokeError();
                    _logger?.LogWarning("Close socket returned error code {ErrorCode} for {InterfaceName}", errorCode, _interfaceName);
                }
                
                _socketHandle.Dispose();
            }
            catch (Exception ex)
            {
                _logger?.LogWarning(ex, "Error disposing socket handle wrapper");
            }
            _socketHandle = null;
        }

        GC.SuppressFinalize(this);
    }
}