BQP/srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/Drivers/PhenikaaX/CiA402Helper.cs

namespace RobotNet10.RobotApp.Drivers.PhenikaaX;

public static class CiA402Helper
{

    /// <summary>
    /// Extract value từ PDO data theo bit offset và bit length
    /// </summary>
    public static byte[] ExtractValueFromPdoData(byte[] pdoData, int bitOffset, byte bitLength)
    {
        int byteOffset = bitOffset / 8;
        int bitOffsetInByte = bitOffset % 8;
        int byteLength = (bitLength + 7) / 8; // Round up

        if (byteOffset + byteLength > pdoData.Length)
        {
            throw new ArgumentException($"PDO data too short for extraction at bit offset {bitOffset}, length {bitLength}");
        }

        byte[] result = new byte[byteLength];

        if (bitOffsetInByte == 0 && bitLength % 8 == 0)
        {
            // Aligned extraction - simple copy
            Array.Copy(pdoData, byteOffset, result, 0, byteLength);
        }
        else
        {
            // Unaligned extraction - need bit manipulation
            ulong value = 0;
            int bitsRead = 0;
            int currentByteOffset = byteOffset;
            int currentBitOffset = bitOffsetInByte;

            while (bitsRead < bitLength && currentByteOffset < pdoData.Length)
            {
                int bitsToRead = Math.Min(8 - currentBitOffset, bitLength - bitsRead);
                byte mask = (byte)((1 << bitsToRead) - 1);
                byte byteValue = (byte)((pdoData[currentByteOffset] >> currentBitOffset) & mask);

                value |= (ulong)byteValue << bitsRead;

                bitsRead += bitsToRead;
                currentByteOffset++;
                currentBitOffset = 0;
            }

            // Convert ulong to byte array
            for (int i = 0; i < byteLength; i++)
            {
                result[i] = (byte)(value >> (i * 8));
            }
        }

        return result;
    }


    /// <summary>
    /// Parse object index từ string (hỗ trợ hex format: 0x6040 hoặc decimal: 24640)
    /// </summary>
    public static bool TryParseObjectIndex(string indexStr, out ushort index)
    {
        index = 0;
        if (string.IsNullOrWhiteSpace(indexStr))
            return false;

        indexStr = indexStr.Trim();
        if (indexStr.StartsWith("0x", StringComparison.OrdinalIgnoreCase))
        {
            return ushort.TryParse(indexStr.AsSpan(2), System.Globalization.NumberStyles.HexNumber, null, out index);
        }
        else
        {
            return ushort.TryParse(indexStr, out index);
        }
    }

    /// <summary>
    /// Convert bytes to value type với sign extension nếu cần
    /// </summary>
    public static T ConvertBytesToValue<T>(byte[] bytes, byte bitLength) where T : struct
    {
        ulong value = 0;
        for (int i = 0; i < Math.Min(bytes.Length, 8); i++)
        {
            value |= (ulong)bytes[i] << (i * 8);
        }

        var type = typeof(T);
        if (type == typeof(ushort))
            return (T)(object)(ushort)value;
        if (type == typeof(short))
        {
            if (bitLength < 16 && (value & (1UL << (bitLength - 1))) != 0)
                value |= 0xFFFFUL << bitLength;
            return (T)(object)(short)value;
        }
        if (type == typeof(uint))
            return (T)(object)(uint)value;
        if (type == typeof(int))
        {
            if (bitLength < 32 && (value & (1UL << (bitLength - 1))) != 0)
                value |= 0xFFFFFFFFUL << bitLength;
            return (T)(object)(int)value;
        }
        if (type == typeof(byte))
            return (T)(object)(byte)value;
        if (type == typeof(sbyte))
        {
            if (bitLength < 8 && (value & (1UL << (bitLength - 1))) != 0)
                value |= 0xFFUL << bitLength;
            return (T)(object)(sbyte)value;
        }

        throw new NotSupportedException($"Type {type.Name} is not supported");
    }

    /// <summary>
    /// Convert value to bytes theo bitLength được map trong PDO
    /// Mục đích của bitLength:
    /// 1. Truncate/mask giá trị nếu vượt quá số bits được map
    /// 2. Đảm bảo chỉ gửi đúng số bytes cần thiết
    /// 3. Xử lý các trường hợp partial mapping (ví dụ: map 8 bits của ushort 16 bits)
    /// </summary>
    public static byte[] ConvertValueToBytes<T>(T value, byte bitLength) where T : struct
    {
        var type = typeof(T);
        int byteLength = (bitLength + 7) / 8; // Round up to nearest byte

        // Convert value to ulong để xử lý mask
        ulong rawValue = 0;
        if (type == typeof(ushort))
            rawValue = (ushort)(object)value!;
        else if (type == typeof(short))
            rawValue = (ulong)(ushort)(short)(object)value!; // Convert signed to unsigned
        else if (type == typeof(uint))
            rawValue = (uint)(object)value!;
        else if (type == typeof(int))
            rawValue = (ulong)(uint)(int)(object)value!; // Convert signed to unsigned
        else if (type == typeof(byte))
            rawValue = (byte)(object)value!;
        else if (type == typeof(sbyte))
            rawValue = (ulong)(byte)(sbyte)(object)value!;
        else
            throw new NotSupportedException($"Type {type.Name} is not supported");

        // Mask để chỉ lấy số bits được map
        if (bitLength < 64)
        {
            ulong mask = (1UL << bitLength) - 1;
            rawValue &= mask;
        }

        // Convert to byte array với đúng số bytes
        byte[] result = new byte[byteLength];
        for (int i = 0; i < byteLength && i < 8; i++)
        {
            result[i] = (byte)(rawValue >> (i * 8));
        }

        return result;
    }

}