I150/srcs/RobotNet10/RobotApp/Communication/RobotNet10.CANOpen/Models/SdoMessage.cs

using RobotNet10.CANOpen.Enums;

namespace RobotNet10.CANOpen.Models;

public readonly struct SdoRequest
{
    public byte CommandSpecifier { get; init; }
    public ushort Index { get; init; }
    public byte SubIndex { get; init; }
    public uint Data { get; init; }
    
    public SdoRequest(SdoCommand command, ushort index, byte subIndex, uint data = 0)
    {
        CommandSpecifier = (byte)command;
        Index = index;
        SubIndex = subIndex;
        Data = data;
    }
    
    public SdoRequest(SdoCommand command, ObjectDictionaryAddress address, uint data = 0)
        : this(command, address.Index, address.SubIndex, data)
    {
    }
    
    public byte[] ToBytes()
    {
        var bytes = new byte[8];
        bytes[0] = CommandSpecifier;
        bytes[1] = (byte)(Index & 0xFF);
        bytes[2] = (byte)((Index >> 8) & 0xFF);
        bytes[3] = SubIndex;
        bytes[4] = (byte)(Data & 0xFF);
        bytes[5] = (byte)((Data >> 8) & 0xFF);
        bytes[6] = (byte)((Data >> 16) & 0xFF);
        bytes[7] = (byte)((Data >> 24) & 0xFF);
        return bytes;
    }
    
    public static SdoRequest CreateUpload(ushort index, byte subIndex)
        => new(SdoCommand.UploadInitiate, index, subIndex);
    
    public static SdoRequest CreateDownload(ushort index, byte subIndex, byte[] data)
    {
        if (data.Length > 4)
            throw new ArgumentException("Use CreateDownloadInitiate for data > 4 bytes");
        
        byte cs = (byte)((byte)SdoCommand.DownloadInitiate | 0x03);
        int n = 4 - data.Length;
        cs |= (byte)((n << 2) & 0x0C);
        
        uint dataValue = 0;
        for (int i = 0; i < data.Length; i++)
            dataValue |= (uint)(data[i] << (i * 8));
        
        return new SdoRequest { CommandSpecifier = cs, Index = index, SubIndex = subIndex, Data = dataValue };
    }
    
    /// <summary>
    /// Create download initiate request for segmented transfer (data > 4 bytes)
    /// </summary>
    public static SdoRequest CreateDownloadInitiate(ushort index, byte subIndex, int dataLength)
    {
        if (dataLength <= 4)
            throw new ArgumentException("Use CreateDownload for data <= 4 bytes");
        
        // Download Initiate: bit 0 = 0 (not expedited), bit 1 = 1 (size indicated), bit 2 = 0 (not complete)
        byte cs = (byte)SdoCommand.DownloadInitiate;
        cs |= 0x02; // Size indicated
        
        // Data contains size (lower 3 bytes) and number of segments not used (upper byte = 0)
        uint sizeData = (uint)dataLength;
        
        return new SdoRequest { CommandSpecifier = cs, Index = index, SubIndex = subIndex, Data = sizeData };
    }
    
    /// <summary>
    /// Create download segment request
    /// </summary>
    /// <param name="toggle">Toggle bit (0 or 1), alternates for each segment</param>
    /// <param name="isLastSegment">True if this is the last segment</param>
    /// <param name="segmentData">Data for this segment (up to 7 bytes)</param>
    public static SdoRequest CreateDownloadSegment(byte toggle, bool isLastSegment, byte[] segmentData)
    {
        if (segmentData.Length > 7)
            throw new ArgumentException("Segment data cannot exceed 7 bytes");
        
        byte cs = (byte)SdoCommand.DownloadSegment;
        if (toggle != 0)
            cs |= 0x10; // Toggle bit
        
        if (isLastSegment)
            cs |= 0x01; // Last segment bit
        
        int n = 7 - segmentData.Length;
        cs |= (byte)((n << 1) & 0x0E);
        
        uint dataValue = 0;
        for (int i = 0; i < segmentData.Length; i++)
            dataValue |= (uint)(segmentData[i] << (i * 8));
        
        return new SdoRequest { CommandSpecifier = cs, Index = 0, SubIndex = 0, Data = dataValue };
    }
    
    /// <summary>
    /// Create upload segment request
    /// </summary>
    /// <param name="toggle">Toggle bit (0 or 1), alternates for each segment</param>
    public static SdoRequest CreateUploadSegment(byte toggle)
    {
        byte cs = (byte)SdoCommand.UploadSegment;
        if (toggle != 0)
            cs |= 0x10; // Toggle bit
        
        return new SdoRequest { CommandSpecifier = cs, Index = 0, SubIndex = 0, Data = 0 };
    }
}

public readonly struct SdoResponse
{
    public byte CommandSpecifier { get; init; }
    public ushort Index { get; init; }
    public byte SubIndex { get; init; }
    public uint Data { get; init; }
    
    public bool IsAbort => (CommandSpecifier & 0xE0) == (byte)SdoCommand.Abort;
    public SdoAbortCode AbortCode => (SdoAbortCode)Data;
    
    public static SdoResponse FromBytes(byte[] data)
    {
        if (data.Length < 8)
            throw new ArgumentException("SDO response must be 8 bytes");
        
        return new SdoResponse
        {
            CommandSpecifier = data[0],
            Index = (ushort)(data[1] | (data[2] << 8)),
            SubIndex = data[3],
            Data = (uint)(data[4] | (data[5] << 8) | (data[6] << 16) | (data[7] << 24))
        };
    }
    
    public byte[] GetDataBytes()
    {
        if ((CommandSpecifier & 0x02) != 0)
        {
            int n = (CommandSpecifier >> 2) & 0x03;
            int dataLength = 4 - n;
            
            var bytes = new byte[dataLength];
            for (int i = 0; i < dataLength; i++)
                bytes[i] = (byte)((Data >> (i * 8)) & 0xFF);
            
            return bytes;
        }
        
        return BitConverter.GetBytes(Data);
    }
    
    /// <summary>
    /// Check if response is expedited transfer
    /// </summary>
    public bool IsExpedited => (CommandSpecifier & 0x02) != 0;
    
    /// <summary>
    /// Check if response indicates size (for segmented transfer)
    /// </summary>
    public bool IsSizeIndicated => (CommandSpecifier & 0x02) != 0;
    
    /// <summary>
    /// Get data size from upload initiate response (for segmented transfer)
    /// </summary>
    public int GetDataSize()
    {
        if (!IsSizeIndicated)
            return 4; // Expedited transfer
        
        return (int)(Data & 0xFFFFFF); // Lower 3 bytes contain size
    }
    
    /// <summary>
    /// Get segment data from upload segment response
    /// </summary>
    public byte[] GetSegmentData()
    {
        int n = (CommandSpecifier >> 1) & 0x07;
        int dataLength = 7 - n;
        
        var bytes = new byte[dataLength];
        for (int i = 0; i < dataLength; i++)
            bytes[i] = (byte)((Data >> (i * 8)) & 0xFF);
        
        return bytes;
    }
    
    /// <summary>
    /// Check if this is the last segment
    /// </summary>
    public bool IsLastSegment => (CommandSpecifier & 0x01) != 0;
    
    /// <summary>
    /// Get toggle bit from segment response
    /// </summary>
    public byte GetToggle() => (byte)((CommandSpecifier >> 4) & 0x01);
}