FirmataDevice.cs

// Licensed to the .NET Foundation under one or more agreements.
// The .NET Foundation licenses this file to you under the MIT license.

using System;
using System.Collections;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Device.Gpio;
using System.Device.Spi;
using System.Diagnostics;
using System.IO;
using System.Linq;
using System.Runtime.InteropServices;
using System.Text;
using System.Threading;
using System.Threading.Tasks;
using Iot.Device.Common;
using Microsoft.Extensions.Logging;
using UnitsNet;

namespace Iot.Device.Arduino
{
    internal delegate void AnalogPinValueUpdated(int pin, uint rawValue);

    /// <summary>
    /// Low-level communication layer for the firmata protocol. Creates the binary command stream for the different commands and returns back results.
    /// </summary>
    internal sealed class FirmataDevice : IDisposable
    {
        private const byte FIRMATA_PROTOCOL_MAJOR_VERSION = 2;
        private const byte FIRMATA_PROTOCOL_MINOR_VERSION = 5; // 2.5 works, but 2.6 is recommended
        private const int FIRMATA_INIT_TIMEOUT_SECONDS = 2;
        internal static readonly TimeSpan DefaultReplyTimeout = TimeSpan.FromMilliseconds(3000);

        private byte _firmwareVersionMajor;
        private byte _firmwareVersionMinor;
        private Version _actualFirmataProtocolVersion;
        private Version _firmwareVersion;

        private int _lastRequestId;

        private string _firmwareName;
        private Stream? _firmataStream;
        private Thread? _inputThread;
        private List<SupportedPinConfiguration> _supportedPinConfigurations;
        private BlockingConcurrentBag<byte[]> _pendingResponses;
        private List<PinValue> _lastPinValues;
        private Dictionary<int, uint> _lastAnalogValues;
        private object _lastPinValueLock;
        private object _lastAnalogValueLock;
        private object _synchronisationLock;
        private Queue<byte> _dataQueue;
        private StringBuilder _lastRawLine;

        private CommandError _lastCommandError;

        private int _i2cSequence;

        /// <summary>
        /// Event used when waiting for answers (i.e. after requesting firmware version)
        /// </summary>
        private AutoResetEvent _dataReceived;

        private ILogger _logger;

        public event PinChangeEventHandler? DigitalPortValueUpdated;

        public event AnalogPinValueUpdated? AnalogPinValueUpdated;

        public event Action<string, Exception?>? OnError;

        public event Action<ReplyType, byte[]>? OnSysexReply;

        private long _bytesTransmitted = 0;

        private int _numberOfConsecutiveI2cWrites = 0;

        private bool _systemVariablesSupported = false;

        public FirmataDevice(List<SupportedMode> supportedModes)
        {
            _firmwareVersionMajor = 0;
            _firmwareVersionMinor = 0;
            _firmwareVersion = new Version(0, 0);
            _actualFirmataProtocolVersion = new Version(0, 0);
            _firmataStream = null;
            InputThreadShouldExit = false;
            _dataReceived = new AutoResetEvent(false);
            _supportedPinConfigurations = new List<SupportedPinConfiguration>();
            _synchronisationLock = new object();
            _lastPinValues = new List<PinValue>();
            _lastPinValueLock = new object();
            _lastAnalogValues = new Dictionary<int, uint>();
            _lastAnalogValueLock = new object();
            _dataQueue = new Queue<byte>(1024);
            _pendingResponses = new BlockingConcurrentBag<byte[]>();
            _lastRequestId = 1;
            _lastCommandError = CommandError.None;
            _firmwareName = string.Empty;
            _lastRawLine = new StringBuilder();
            SupportedModes = supportedModes;
            _i2cSequence = 0;
            _logger = this.GetCurrentClassLogger();
        }

        internal List<SupportedPinConfiguration> PinConfigurations
        {
            get
            {
                return _supportedPinConfigurations;
            }
        }

        internal List<SupportedMode> SupportedModes { get; set; }

        internal long BytesTransmitted => _bytesTransmitted;

        internal bool InputThreadShouldExit { get; set; }

        public void Open(Stream stream)
        {
            lock (_synchronisationLock)
            {
                if (_firmataStream != null)
                {
                    throw new InvalidOperationException("The device is already open");
                }

                _firmataStream = stream;
                if (_firmataStream.CanRead && _firmataStream.CanWrite)
                {
                    StartListening();
                }
                else
                {
                    throw new NotSupportedException("Need a read-write stream to the hardware device");
                }
            }
        }

        private void StartListening()
        {
            if (_firmataStream == null)
            {
                throw new ObjectDisposedException(nameof(FirmataDevice));
            }

            if (_inputThread != null && _inputThread.IsAlive)
            {
                return;
            }

            InputThreadShouldExit = false;

            _inputThread = new Thread(InputThread);
            _inputThread.Name = "Firmata input thread";
            _inputThread.Start();
        }

        private void ProcessInput()
        {
            if (_dataQueue.Count == 0)
            {
                FillQueue();
            }

            if (_dataQueue.Count == 0)
            {
                // Still no data? (End of stream or stream closed)
                return;
            }

            int data = _dataQueue.Dequeue();

            // OnError?.Invoke($"0x{data:X}", null);
            byte b = (byte)(data & 0x00FF);
            byte upper_nibble = (byte)(data & 0xF0);
            byte lower_nibble = (byte)(data & 0x0F);

            /*
             * the relevant bits in the command depends on the value of the data byte. If it is less than 0xF0 (start sysex), only the upper nibble identifies the command
             * while the lower nibble contains additional data
             */
            FirmataCommand command = (FirmataCommand)((data < ((ushort)FirmataCommand.START_SYSEX) ? upper_nibble : b));

            // determine the number of bytes remaining in the message
            int bytes_remaining = 0;
            bool isMessageSysex = false;
            switch (command)
            {
                default: // command not understood
                    char c = (char)data;
                    _lastRawLine.Append(c);
                    if (c == '\n')
                    {
                        OnError?.Invoke(_lastRawLine.ToString().Trim(), null);
                        OnSysexReply?.Invoke(ReplyType.AsciiData, Encoding.Unicode.GetBytes(_lastRawLine.ToString()));
                        _lastRawLine.Clear();
                    }

                    return;
                case FirmataCommand.END_SYSEX: // should never happen
                    return;

                // commands that require 2 additional bytes
                case FirmataCommand.DIGITAL_MESSAGE:
                case FirmataCommand.ANALOG_MESSAGE:
                case FirmataCommand.SET_PIN_MODE:
                case FirmataCommand.PROTOCOL_VERSION:
                    bytes_remaining = 2;
                    break;

                // commands that require 1 additional byte
                case FirmataCommand.REPORT_ANALOG_PIN:
                case FirmataCommand.REPORT_DIGITAL_PIN:
                    bytes_remaining = 1;
                    break;

                // commands that do not require additional bytes
                case FirmataCommand.SYSTEM_RESET:
                    // do nothing, as there is nothing to reset
                    return;

                case FirmataCommand.START_SYSEX:
                    // this is a special case with no set number of bytes remaining
                    isMessageSysex = true;
                    _lastRawLine.Clear();
                    break;
            }

            // read the remaining message while keeping track of elapsed time to timeout in case of incomplete message
            List<byte> message = new List<byte>();
            int bytes_read = 0;
            Stopwatch timeout_start = Stopwatch.StartNew();
            while (bytes_remaining > 0 || isMessageSysex)
            {
                if (_dataQueue.Count == 0)
                {
                    int timeout = 10;
                    while (!FillQueue() && timeout-- > 0)
                    {
                        Thread.Sleep(5);
                    }

                    if (timeout == 0)
                    {
                        // Synchronisation problem: The remainder of the expected message is missing
                        _lastCommandError = CommandError.Timeout;
                        return;
                    }
                }

                data = _dataQueue.Dequeue();
                // OnError?.Invoke($"0x{data:X}", null);
                // if no data was available, check for timeout
                if (data == 0xFFFF)
                {
                    // get elapsed seconds, given as a double with resolution in nanoseconds
                    TimeSpan elapsed = timeout_start.Elapsed;

                    if (elapsed > DefaultReplyTimeout)
                    {
                        _lastCommandError = CommandError.Timeout;
                        return;
                    }

                    continue;
                }

                timeout_start.Restart();

                // if we're parsing sysex and we've just read the END_SYSEX command, we're done.
                if (isMessageSysex && (data == (short)FirmataCommand.END_SYSEX))
                {
                    break;
                }

                message.Add((byte)(data & 0xFF));
                ++bytes_read;
                --bytes_remaining;
            }

            // process the message
            switch (command)
            {
                // ignore these message types (they should not be in a reply)
                default:
                case FirmataCommand.REPORT_ANALOG_PIN:
                case FirmataCommand.REPORT_DIGITAL_PIN:
                case FirmataCommand.SET_PIN_MODE:
                case FirmataCommand.END_SYSEX:
                case FirmataCommand.SYSTEM_RESET:
                    return;
                case FirmataCommand.PROTOCOL_VERSION:
                    if (_actualFirmataProtocolVersion.Major != 0)
                    {
                        // Firmata sends this message automatically after a device reset (if you press the reset button on the arduino)
                        // If we know the version already, this is unexpected.
                        _lastCommandError = CommandError.DeviceReset;
                        OnError?.Invoke("The device was unexpectedly reset. Please restart the communication.", null);
                    }

                    _actualFirmataProtocolVersion = new Version(message[0], message[1]);
                    _logger.LogInformation($"Received protocol version: {_actualFirmataProtocolVersion}.");
                    _dataReceived.Set();

                    return;

                case FirmataCommand.ANALOG_MESSAGE:
                    // report analog commands store the pin number in the lower nibble of the command byte, the value is split over two 7-bit bytes
                    {
                        int channel = lower_nibble;
                        uint value = (uint)(message[0] | (message[1] << 7));
                        // This must work
                        int pin = _supportedPinConfigurations.First(x => x.AnalogPinNumber == channel).Pin;
                        lock (_lastAnalogValueLock)
                        {
                            _lastAnalogValues[pin] = value;
                        }

                        AnalogPinValueUpdated?.Invoke(channel, value);
                    }

                    break;

                case FirmataCommand.DIGITAL_MESSAGE:
                    // digital messages store the port number in the lower nibble of the command byte, the port value is split over two 7-bit bytes
                    // Each port corresponds to 8 pins
                    {
                        int offset = lower_nibble * 8;
                        ushort pinValues = (ushort)(message[0] | (message[1] << 7));
                        if (offset + 7 >= _lastPinValues.Count)
                        {
                            _logger.LogError($"Firmware reported an update for port {lower_nibble}, but there are only {_supportedPinConfigurations.Count} pins");
                            break;
                        }

                        lock (_lastPinValueLock)
                        {
                            for (int i = 0; i < 8; i++)
                            {
                                PinValue oldValue = _lastPinValues[i + offset];
                                int mask = 1 << i;
                                PinValue newValue = (pinValues & mask) == 0 ? PinValue.Low : PinValue.High;
                                if (newValue != oldValue)
                                {
                                    PinEventTypes eventTypes = newValue == PinValue.High ? PinEventTypes.Rising : PinEventTypes.Falling;
                                    _lastPinValues[i + offset] = newValue;
                                    // TODO: The callback should not be within the lock
                                    DigitalPortValueUpdated?.Invoke(this, new PinValueChangedEventArgs(eventTypes, i + offset));
                                }
                            }
                        }
                    }

                    break;

                case FirmataCommand.START_SYSEX:
                    // a sysex message must include at least one extended-command byte
                    if (bytes_read < 1)
                    {
                        _lastCommandError = CommandError.InvalidArguments;
                        return;
                    }

                    // retrieve the raw data array & extract the extended-command byte
                    byte[] raw_data = message.ToArray();
                    FirmataSysexCommand sysCommand = (FirmataSysexCommand)(raw_data[0]);
                    int index = 0;
                    ++index;
                    --bytes_read;

                    switch (sysCommand)
                    {
                        case FirmataSysexCommand.REPORT_FIRMWARE:
                            // See https://github.com/firmata/protocol/blob/master/protocol.md
                            // Byte 0 is the command (0x79) and can be skipped here, as we've already interpreted it
                            {
                                _firmwareVersionMajor = raw_data[1];
                                _firmwareVersionMinor = raw_data[2];
                                _firmwareVersion = new Version(_firmwareVersionMajor, _firmwareVersionMinor);
                                int stringLength = (raw_data.Length - 3) / 2;
                                Span<byte> bytesReceived = stackalloc byte[stringLength];
                                ReassembleByteString(raw_data, 3, stringLength * 2, bytesReceived);

                                _firmwareName = Encoding.ASCII.GetString(bytesReceived);
                                _logger.LogDebug($"Received Firmware name {_firmwareName}");
                                _dataReceived.Set();
                            }

                            return;

                        case FirmataSysexCommand.STRING_DATA:
                            {
                                // condense back into 1-byte data
                                int stringLength = (raw_data.Length - 1) / 2;
                                Span<byte> bytesReceived = stackalloc byte[stringLength];
                                ReassembleByteString(raw_data, 1, stringLength * 2, bytesReceived);

                                string message1 = Encoding.UTF8.GetString(bytesReceived);
                                int idxNull = message1.IndexOf('\0');
                                if (message1.Contains("%") && idxNull > 0) // C style printf formatters
                                {
                                    message1 = message1.Substring(0, idxNull);
                                    string message2 = PrintfFromByteStream(message1, bytesReceived, idxNull + 1);
                                    OnError?.Invoke(message2, null);
                                }
                                else
                                {
                                    OnError?.Invoke(message1, null);
                                }
                            }

                            break;

                        case FirmataSysexCommand.CAPABILITY_RESPONSE:
                            {
                                _supportedPinConfigurations.Clear();
                                int idx = 1;
                                SupportedPinConfiguration currentPin = new SupportedPinConfiguration(0);
                                int pin = 0;
                                while (idx < raw_data.Length)
                                {
                                    int mode = raw_data[idx++];
                                    if (mode == 0x7F)
                                    {
                                        _supportedPinConfigurations.Add(currentPin);
                                        currentPin = new SupportedPinConfiguration(++pin);
                                        continue;
                                    }

                                    int resolution = raw_data[idx++];
                                    SupportedMode? sm = SupportedModes.FirstOrDefault(x => x.Value == mode);
                                    if (sm == SupportedMode.AnalogInput)
                                    {
                                        currentPin.PinModes.Add(SupportedMode.AnalogInput);
                                        currentPin.AnalogInputResolutionBits = resolution;
                                    }
                                    else if (sm == SupportedMode.Pwm)
                                    {
                                        currentPin.PinModes.Add(SupportedMode.Pwm);
                                        currentPin.PwmResolutionBits = resolution;
                                    }
                                    else if (sm == null)
                                    {
                                        sm = new SupportedMode((byte)mode, $"Unknown mode {mode}");
                                        currentPin.PinModes.Add(sm);
                                    }
                                    else
                                    {
                                        currentPin.PinModes.Add(sm);
                                    }
                                }

                                // Add 8 entries, so that later we do not need to check whether a port (bank) is complete
                                _lastPinValues = new PinValue[_supportedPinConfigurations.Count + 8].ToList();
                                _dataReceived.Set();
                                // Do not add the last instance, should also be terminated by 0xF7
                            }

                            break;

                        case FirmataSysexCommand.ANALOG_MAPPING_RESPONSE:
                            {
                                // This needs to have been set up previously
                                if (_supportedPinConfigurations.Count == 0)
                                {
                                    return;
                                }

                                int idx = 1;
                                int pin = 0;
                                while (idx < raw_data.Length)
                                {
                                    if (raw_data[idx] != 127)
                                    {
                                        _supportedPinConfigurations[pin].AnalogPinNumber = raw_data[idx];
                                    }

                                    idx++;
                                    pin++;
                                }

                                _dataReceived.Set();
                            }

                            break;

                        case FirmataSysexCommand.EXTENDED_ANALOG:
                            // report analog commands store the pin number in the lower nibble of the command byte, the value is split over two 7-bit bytes
                            {
                                int channel = raw_data[1];
                                uint value = (uint)(raw_data[2] | (raw_data[3] << 7));
                                // This must work
                                int pin = _supportedPinConfigurations.First(x => x.AnalogPinNumber == channel).Pin;
                                lock (_lastAnalogValueLock)
                                {
                                    _lastAnalogValues[pin] = value;
                                }

                                AnalogPinValueUpdated?.Invoke(channel, value);
                            }

                            break;

                        case FirmataSysexCommand.I2C_REPLY:
                            _lastCommandError = CommandError.None;
                            _pendingResponses.Add(raw_data);
                            break;

                        case FirmataSysexCommand.SPI_DATA:
                            _lastCommandError = CommandError.None;
                            _pendingResponses.Add(raw_data);
                            break;

                        default:
                            // we pass the data forward as-is for any other type of sysex command
                            _lastCommandError = CommandError.None;
                            _pendingResponses.Add(raw_data);
                            OnSysexReply?.Invoke(ReplyType.SysexCommand, raw_data);
                            break;
                    }

                    break;
            }
        }

        /// <summary>
        /// Send a command that does not generate a reply.
        /// This method must only be used for commands that do not generate a reply. It must not be used if only the caller is not
        /// interested in the answer.
        /// </summary>
        /// <param name="sequence">The command sequence to send</param>
        public void SendCommand(FirmataCommandSequence sequence)
        {
            if (!sequence.Validate())
            {
                throw new ArgumentException("The command sequence is invalid", nameof(sequence));
            }

            lock (_synchronisationLock)
            {
                if (_firmataStream == null)
                {
                    throw new ObjectDisposedException(nameof(FirmataDevice));
                }

                _firmataStream.Write(sequence.Sequence.ToArray());
                _bytesTransmitted += sequence.Sequence.Count;
                _firmataStream.Flush();
            }
        }

        /// <summary>
        /// Send a command and wait for a reply
        /// </summary>
        /// <param name="sequence">The command sequence, typically starting with <see cref="FirmataCommand.START_SYSEX"/> and ending with <see cref="FirmataCommand.END_SYSEX"/></param>
        /// <param name="timeout">A non-default timeout</param>
        /// <param name="isMatchingAck">A callback function that should return true if the given reply is the one this command should wait for. The default is true, because asynchronous replies
        /// are rather the exception than the rule</param>
        /// <param name="error">An error code in case of failure</param>
        /// <returns>The raw sequence of sysex reply bytes. The reply does not include the START_SYSEX byte, but it does include the terminating END_SYSEX byte. The first byte is the
        /// <see cref="FirmataSysexCommand"/> command number of the corresponding request</returns>
        public byte[] SendCommandAndWait(FirmataCommandSequence sequence, TimeSpan timeout, Func<FirmataCommandSequence, byte[], bool> isMatchingAck, out CommandError error)
        {
            if (!sequence.Validate())
            {
                throw new ArgumentException("The command sequence is invalid", nameof(sequence));
            }

            if (_firmataStream == null)
            {
                throw new ObjectDisposedException(nameof(FirmataDevice));
            }

            _firmataStream.Write(sequence.Sequence.ToArray(), 0, sequence.Sequence.Count);
            _bytesTransmitted += sequence.Sequence.Count;
            _firmataStream.Flush();

            byte[]? response;
            if (!_pendingResponses.TryRemoveElement(x => isMatchingAck(sequence, x!), timeout, out response))
            {
                throw new TimeoutException("Timeout waiting for command answer");
            }

            error = _lastCommandError;
            return response ?? throw new InvalidOperationException("Got a null reply"); // should not happen in our case
        }

        /// <summary>
        /// Send a set of command and wait for a reply
        /// </summary>
        /// <param name="sequences">The command sequences to send, typically starting with <see cref="FirmataCommand.START_SYSEX"/> and ending with <see cref="FirmataCommand.END_SYSEX"/></param>
        /// <param name="timeout">A non-default timeout</param>
        /// <param name="isMatchingAck">A callback function that should return true if the given reply is the one this command should wait for. The default is true, because asynchronous replies
        /// are rather the exception than the rule</param>
        /// <param name="errorFunc">A callback that determines a possible error in the reply message</param>
        /// <param name="error">An error code in case of failure</param>
        /// <returns>The raw sequence of sysex reply bytes. The reply does not include the START_SYSEX byte, but it does include the terminating END_SYSEX byte. The first byte is the
        /// <see cref="FirmataSysexCommand"/> command number of the corresponding request</returns>
        public bool SendCommandsAndWait(IList<FirmataCommandSequence> sequences, TimeSpan timeout, Func<FirmataCommandSequence, byte[], bool> isMatchingAck,
            Func<FirmataCommandSequence, byte[], CommandError> errorFunc, out CommandError error)
        {
            if (sequences.Any(s => s.Validate() == false))
            {
                throw new ArgumentException("At least one command sequence is invalid", nameof(sequences));
            }

            if (sequences.Count > 127)
            {
                // Because we only have 7 bits for the sequence counter.
                throw new ArgumentException("At most 127 sequences can be chained together", nameof(sequences));
            }

            if (isMatchingAck == null)
            {
                throw new ArgumentNullException(nameof(isMatchingAck));
            }

            error = CommandError.None;
            if (_firmataStream == null)
            {
                throw new ObjectDisposedException(nameof(FirmataDevice));
            }

            Dictionary<FirmataCommandSequence, bool> sequencesWithAck = new();
            foreach (FirmataCommandSequence s in sequences)
            {
                sequencesWithAck.Add(s, false);
                _firmataStream.Write(s.InternalSequence, 0, s.Length);
            }

            _firmataStream.Flush();

            byte[]? response;
            do
            {
                foreach (KeyValuePair<FirmataCommandSequence, bool> s2 in sequencesWithAck)
                {
                    if (s2.Value == false && _pendingResponses.TryRemoveElement(x => isMatchingAck(s2.Key, x!), timeout, out response))
                    {
                        CommandError e = CommandError.None;
                        if (response == null)
                        {
                            error = CommandError.Aborted;
                        }
                        else if (_lastCommandError != CommandError.None)
                        {
                            error = _lastCommandError;
                        }
                        else if ((e = errorFunc(s2.Key, response)) != CommandError.None)
                        {
                            error = e;
                        }

                        sequencesWithAck[s2.Key] = true;
                        break;
                    }
                }
            }
            while (sequencesWithAck.Any(x => x.Value == false));

            return sequencesWithAck.All(x => x.Value);
        }

        /// <summary>
        /// Replaces the first occurrence of search in input with replace.
        /// </summary>
        private static string ReplaceFirst(String input, string search, string replace)
        {
            int idx = input.IndexOf(search, StringComparison.InvariantCulture);
            string output = input.Remove(idx, search.Length);
            output = output.Insert(idx, replace);
            return output;
        }

        /// <summary>
        /// Simulates a printf C statement.
        /// Note that the word size on the arduino is 16 bits, so any argument not specifying an l prefix is considered to
        /// be 16 bits only.
        /// </summary>
        /// <param name="fmt">Format string (with %d, %x, etc)</param>
        /// <param name="bytesReceived">Total bytes received</param>
        /// <param name="startOfArguments">Start of arguments (first byte of formatting parameters)</param>
        /// <returns>A formatted string</returns>
        private string PrintfFromByteStream(string fmt, in Span<byte> bytesReceived, int startOfArguments)
        {
            string output = fmt;
            while (output.Contains("%"))
            {
                int idxPercent = output.IndexOf('%');
                string type = output[idxPercent + 1].ToString();
                if (type == "l")
                {
                    type += output[idxPercent + 2];
                }

                switch (type)
                {
                    case "lx":
                    {
                        Int32 arg = BitConverter.ToInt32(bytesReceived.ToArray(), startOfArguments);
                        output = ReplaceFirst(output, "%" + type, arg.ToString("x"));
                        startOfArguments += 4;
                        break;
                    }

                    case "x":
                    {
                        Int16 arg = BitConverter.ToInt16(bytesReceived.ToArray(), startOfArguments);
                        output = ReplaceFirst(output, "%" + type, arg.ToString("x"));
                        startOfArguments += 2;
                        break;
                    }

                    case "d":
                    {
                        Int16 arg = BitConverter.ToInt16(bytesReceived.ToArray(), startOfArguments);
                        output = ReplaceFirst(output, "%" + type, arg.ToString());
                        startOfArguments += 2;
                        break;
                    }

                    case "ld":
                    {
                        Int32 arg = BitConverter.ToInt32(bytesReceived.ToArray(), startOfArguments);
                        output = ReplaceFirst(output, "%" + type, arg.ToString());
                        startOfArguments += 4;
                        break;
                    }
                }
            }

            return output;
        }

        private bool FillQueue()
        {
            if (_firmataStream == null)
            {
                throw new ObjectDisposedException(nameof(FirmataDevice));
            }

            Span<byte> rawData = stackalloc byte[512];

            int bytesRead = _firmataStream.Read(rawData);
            for (int i = 0; i < bytesRead; i++)
            {
                _dataQueue.Enqueue(rawData[i]);
            }

            return _dataQueue.Count > 0;
        }

        private void InputThread()
        {
            while (!InputThreadShouldExit)
            {
                try
                {
                    ProcessInput();
                }
                catch (Exception ex)
                {
                    // If the exception happens because the stream was closed, don't print an error
                    if (!InputThreadShouldExit)
                    {
                        _logger.LogError(ex, $"Error in parser: {ex.Message}");
                        OnError?.Invoke($"Firmata protocol error: Parser exception {ex.Message}", ex);
                    }
                }
            }
        }

        public Version QueryFirmataVersion()
        {
            if (_firmataStream == null)
            {
                throw new ObjectDisposedException(nameof(FirmataDevice));
            }

            // Try a few times (because we have to make sure the receiver's input queue is properly synchronized and the device
            // has properly booted)
            for (int i = 0; i < 20; i++)
            {
                lock (_synchronisationLock)
                {
                    _dataReceived.Reset();
                    _firmataStream.WriteByte((byte)FirmataCommand.PROTOCOL_VERSION);
                    _firmataStream.Flush();
                    bool result = _dataReceived.WaitOne(TimeSpan.FromSeconds(FIRMATA_INIT_TIMEOUT_SECONDS));
                    if (result == false)
                    {
                        // Attempt to send a SYSTEM_RESET command
                        _firmataStream.WriteByte(0xFF);
                        Thread.Sleep(20);
                        continue;
                    }

                    if (_actualFirmataProtocolVersion.Major == 0)
                    {
                        // The device may be resetting itself as part of opening the serial port (this is the typical
                        // behavior of the Arduino Uno, but not of most newer boards)
                        Thread.Sleep(100);
                        continue;
                    }

                    return _actualFirmataProtocolVersion;
                }
            }

            throw new TimeoutException("Timeout waiting for firmata version");
        }

        internal Version QuerySupportedFirmataVersion()
        {
            return new Version(FIRMATA_PROTOCOL_MAJOR_VERSION, FIRMATA_PROTOCOL_MINOR_VERSION);
        }

        internal Version QueryFirmwareVersion(out string firmwareName)
        {
            if (_firmataStream == null)
            {
                throw new ObjectDisposedException(nameof(FirmataDevice));
            }

            // Try 3 times (because we have to make sure the receiver's input queue is properly synchronized)
            for (int i = 0; i < 3; i++)
            {
                lock (_synchronisationLock)
                {
                    _dataReceived.Reset();
                    _firmataStream.WriteByte((byte)FirmataCommand.START_SYSEX);
                    _firmataStream.WriteByte((byte)FirmataSysexCommand.REPORT_FIRMWARE);
                    _firmataStream.WriteByte((byte)FirmataCommand.END_SYSEX);
                    bool result = _dataReceived.WaitOne(TimeSpan.FromSeconds(FIRMATA_INIT_TIMEOUT_SECONDS));
                    if (result == false || _firmwareVersionMajor == 0)
                    {
                        // Wait a bit until we try again.
                        Thread.Sleep(100);
                        continue;
                    }

                    firmwareName = _firmwareName;
                    return new Version(_firmwareVersionMajor, _firmwareVersionMinor);
                }
            }

            throw new TimeoutException("Timeout waiting for firmata firmware version");
        }

        internal void QueryCapabilities()
        {
            if (_firmataStream == null)
            {
                throw new ObjectDisposedException(nameof(FirmataDevice));
            }

            lock (_synchronisationLock)
            {
                _dataReceived.Reset();
                _firmataStream.WriteByte((byte)FirmataCommand.START_SYSEX);
                _firmataStream.WriteByte((byte)FirmataSysexCommand.CAPABILITY_QUERY);
                _firmataStream.WriteByte((byte)FirmataCommand.END_SYSEX);
                bool result = _dataReceived.WaitOne(DefaultReplyTimeout);
                if (result == false)
                {
                    throw new TimeoutException("Timeout waiting for device capabilities");
                }

                _dataReceived.Reset();
                _firmataStream.WriteByte((byte)FirmataCommand.START_SYSEX);
                _firmataStream.WriteByte((byte)FirmataSysexCommand.ANALOG_MAPPING_QUERY);
                _firmataStream.WriteByte((byte)FirmataCommand.END_SYSEX);
                result = _dataReceived.WaitOne(DefaultReplyTimeout);
                if (result == false)
                {
                    throw new TimeoutException("Timeout waiting for PWM port mappings");
                }
            }
        }

        private void StopThread()
        {
            InputThreadShouldExit = true;
            if (_inputThread != null)
            {
                _inputThread.Join();
                _inputThread = null;
            }
        }

        private T PerformRetries<T>(int numberOfRetries, Func<T> operation)
        {
            Exception? lastException = null;
            while (numberOfRetries-- > 0)
            {
                try
                {
                    T result = operation();
                    return result;
                }
                catch (TimeoutException x)
                {
                    lastException = x;
                    OnError?.Invoke("Timeout waiting for answer. Retries possible.", x);
                    Thread.Sleep(20);
                }
            }

            throw new TimeoutException("Timeout waiting for answer. Aborting. ", lastException);
        }

        internal void SetPinMode(int pin, SupportedMode mode)
        {
            byte firmataMode = mode.Value;
            FirmataCommandSequence s = new FirmataCommandSequence(FirmataCommand.SET_PIN_MODE);
            s.WriteByte((byte)pin);
            s.WriteByte((byte)firmataMode);
            for (int i = 0; i < 3; i++)
            {
                SendCommand(s);

                if (GetPinMode(pin) == firmataMode)
                {
                    return;
                }
            }

            throw new TimeoutException($"Unable to set Pin mode to {firmataMode}.");
        }

        internal byte GetPinMode(int pinNumber)
        {
            FirmataCommandSequence getPinModeSequence = new FirmataCommandSequence(FirmataCommand.START_SYSEX);
            getPinModeSequence.WriteByte((byte)FirmataSysexCommand.PIN_STATE_QUERY);
            getPinModeSequence.WriteByte((byte)pinNumber);
            getPinModeSequence.WriteByte((byte)FirmataCommand.END_SYSEX);

            return PerformRetries(3, () =>
            {
                byte[] response = SendCommandAndWait(getPinModeSequence, DefaultReplyTimeout, (sequence, bytes) =>
                {
                    return bytes.Length >= 4 && bytes[1] == pinNumber;
                }, out _);

                // The mode is byte 4
                if (response.Length < 4)
                {
                    throw new InvalidOperationException("Not enough data in reply");
                }

                if (response[1] != pinNumber)
                {
                    throw new InvalidOperationException(
                        "The reply didn't match the query (another port was indicated)");
                }

                return (response[2]);
            });
        }

        /// <summary>
        /// Enables digital pin reporting for all ports (one port has 8 pins)
        /// </summary>
        internal void EnableDigitalReporting()
        {
            if (_firmataStream == null)
            {
                throw new ObjectDisposedException(nameof(FirmataDevice));
            }

            int numPorts = (int)Math.Ceiling(PinConfigurations.Count / 8.0);
            lock (_synchronisationLock)
            {
                for (byte i = 0; i < numPorts; i++)
                {
                    _firmataStream.WriteByte((byte)(0xD0 + i));
                    _firmataStream.WriteByte(1);
                    _firmataStream.Flush();
                }
            }
        }

        public PinValue ReadDigitalPin(int pinNumber)
        {
            lock (_lastPinValueLock)
            {
                return _lastPinValues[pinNumber];
            }
        }

        internal void WriteDigitalPin(int pin, PinValue value)
        {
            if (_firmataStream == null)
            {
                throw new ObjectDisposedException(nameof(FirmataDevice));
            }

            FirmataCommandSequence writeDigitalPin = new FirmataCommandSequence(FirmataCommand.SET_DIGITAL_VALUE);
            writeDigitalPin.WriteByte((byte)pin);
            writeDigitalPin.WriteByte(value == PinValue.High ? (byte)1 : (byte)0);

            SendCommand(writeDigitalPin);
        }

        public void SendI2cConfigCommand()
        {
            FirmataCommandSequence i2cConfigCommand = new();
            i2cConfigCommand.WriteByte((byte)FirmataSysexCommand.I2C_CONFIG);
            i2cConfigCommand.WriteByte(0);
            i2cConfigCommand.WriteByte(0);
            i2cConfigCommand.WriteByte((byte)FirmataCommand.END_SYSEX);
            SendCommand(i2cConfigCommand);
        }

        public void WriteReadI2cData(int slaveAddress,  ReadOnlySpan<byte> writeData, Span<byte> replyData)
        {
            // See documentation at https://github.com/firmata/protocol/blob/master/i2c.md
            FirmataCommandSequence i2cSequence = new FirmataCommandSequence();
            bool doWait = false;
            if (writeData.Length > 0)
            {
                i2cSequence.WriteByte((byte)FirmataSysexCommand.I2C_REQUEST);
                i2cSequence.WriteByte((byte)slaveAddress);
                // Write flag is 0, all other bits normally, too.
                i2cSequence.WriteByte(0);
                i2cSequence.WriteBytesAsTwo7bitBytes(writeData);
                i2cSequence.WriteByte((byte)FirmataCommand.END_SYSEX);
            }

            int sequenceNo = (_i2cSequence++) & 0b111;

            if (replyData.Length > 0)
            {
                doWait = true;
                if (i2cSequence.Length > 1)
                {
                    // If the above block was executed, we have to insert another START_SYSEX, otherwise it's already there
                    i2cSequence.WriteByte((byte)FirmataCommand.START_SYSEX);
                }

                i2cSequence.WriteByte((byte)FirmataSysexCommand.I2C_REQUEST);
                i2cSequence.WriteByte((byte)slaveAddress);

                // Read flag is 1, all other bits are 0. We use bits 0-2  (slave address MSB, unused in 7 bit mode) as sequence id.
                i2cSequence.WriteByte((byte)(0b1000 | sequenceNo));
                byte length = (byte)replyData.Length;
                // Only write the length of the expected data.
                // We could insert the register to read here, but we assume that has been written already (the client is responsible for that)
                i2cSequence.WriteByte((byte)(length & (uint)sbyte.MaxValue));
                i2cSequence.WriteByte((byte)(length >> 7 & sbyte.MaxValue));
                i2cSequence.WriteByte((byte)FirmataCommand.END_SYSEX);
            }

            if (doWait)
            {
                byte[] response = SendCommandAndWait(i2cSequence, TimeSpan.FromSeconds(10), (sequence, bytes) =>
                {
                    if (bytes.Length < 5)
                    {
                        return false;
                    }

                    if (bytes[0] != (byte)FirmataSysexCommand.I2C_REPLY)
                    {
                        return false;
                    }

                    if ((bytes[2] & 0b111) != sequenceNo)
                    {
                        return false;
                    }

                    return true;
                }, out _);

                if (response[0] != (byte)FirmataSysexCommand.I2C_REPLY)
                {
                    throw new IOException("Firmata protocol error: received incorrect query response");
                }

                if (response[1] != (byte)slaveAddress && slaveAddress != 0)
                {
                    throw new IOException($"Firmata protocol error: The wrong device did answer. Expected {slaveAddress} but got {response[1]}.");
                }

                // Byte 0: I2C_REPLY
                // Bytes 1 & 2: Slave address (the MSB is always 0, since we're only supporting 7-bit addresses)
                // Bytes 3 & 4: Register. Often 0, and probably not needed
                // Anything after that: reply data, with 2 bytes for each byte in the data stream
                int bytesReceived = ReassembleByteString(response, 5, response.Length - 5, replyData);

                if (replyData.Length != bytesReceived)
                {
                    throw new IOException($"Expected {replyData.Length} bytes, got only {bytesReceived}");
                }

                _numberOfConsecutiveI2cWrites = 0; // after we get a reply, we can free-fire a few times again
            }
            else
            {
                SendCommand(i2cSequence);
                // If we use a series of write-only I2C commands we have to occasionally introduce a command that requires an answer, or we flood the device's input buffer,
                // causing network retries, which under the line causes more delays than this. This problem is particularly obvious with an ESP32 in Wifi mode
                _numberOfConsecutiveI2cWrites++;
                if (_numberOfConsecutiveI2cWrites % 4 == 0)
                {
                    var pin = _supportedPinConfigurations.FirstOrDefault(x => x.PinModes.Contains(SupportedMode.I2c));
                    if (pin != null)
                    {
                        GetPinMode(pin.Pin);
                    }
                    else
                    {
                        Thread.Sleep(10); // Hopefully, this doesn't happen
                    }
                }
            }
        }

        public void SetPwmChannel(int pin, double dutyCycle)
        {
            // The arduino expects values between 0 and 255 for PWM channels.
            // The frequency cannot currently be set using the protocol
            int pwmMaxValue = _supportedPinConfigurations[pin].PwmResolutionBits; // This is 8 for most arduino boards
            pwmMaxValue = (1 << pwmMaxValue) - 1;
            int value = (int)Math.Max(0, Math.Min(dutyCycle * pwmMaxValue, pwmMaxValue));

            // At most 14 bits used?
            if (((pwmMaxValue & 0x3FFF) == pwmMaxValue) && (pin <= 15))
            {
                // Use shorthand
                FirmataCommandSequence analogMessage = new FirmataCommandSequence(FirmataCommand.ANALOG_MESSAGE, pin);
                analogMessage.WriteByte((byte)(value & (uint)sbyte.MaxValue)); // lower 7 bits
                analogMessage.WriteByte((byte)(value >> 7 & sbyte.MaxValue)); // top bit (rest unused)
                // No(!) END_SYSEX here
                SendCommand(analogMessage);
                return;
            }

            FirmataCommandSequence pwmCommandSequence = new();
            pwmCommandSequence.WriteByte((byte)FirmataSysexCommand.EXTENDED_ANALOG);
            pwmCommandSequence.WriteByte((byte)pin);
            pwmCommandSequence.WriteByte((byte)(value & (uint)sbyte.MaxValue)); // lower 7 bits
            pwmCommandSequence.WriteByte((byte)(value >> 7 & sbyte.MaxValue)); // top bit (rest unused)
            pwmCommandSequence.WriteByte((byte)FirmataCommand.END_SYSEX);
            SendCommand(pwmCommandSequence);
        }

        /// <summary>
        /// Enable analog reporting for the given physical pin
        /// </summary>
        /// <param name="pinNumber">Physical pin number</param>
        /// <param name="analogChannel">Analog channel corresponding to the given pin (Axx in arduino terminology)</param>
        internal void EnableAnalogReporting(int pinNumber, int analogChannel)
        {
            if (_firmataStream == null)
            {
                throw new ObjectDisposedException(nameof(FirmataDevice));
            }

            lock (_synchronisationLock)
            {
                _lastAnalogValues[pinNumber] = 0; // to make sure this entry exists
                _logger.LogInformation($"Enabling analog reporting on pin {pinNumber}, Channel A{analogChannel}");
                if (analogChannel <= 15)
                {
                    _firmataStream.WriteByte((byte)((int)FirmataCommand.REPORT_ANALOG_PIN + analogChannel));
                    _firmataStream.WriteByte((byte)1);
                }
                else if (_actualFirmataProtocolVersion >= new Version(2, 7))
                {
                    // Note: Requires Protocol Version 2.7 or later
                    FirmataCommandSequence commandSequence = new();
                    commandSequence.WriteByte((byte)FirmataSysexCommand.EXTENDED_REPORT_ANALOG);
                    commandSequence.WriteByte((byte)analogChannel);
                    commandSequence.WriteByte((byte)1);
                    commandSequence.WriteByte((byte)FirmataCommand.END_SYSEX);
                    SendCommand(commandSequence);
                }
                else
                {
                    throw new NotSupportedException($"Using analog channel A{analogChannel} requires firmata protocol version 2.7 or later");
                }
            }
        }

        internal void DisableAnalogReporting(int pinNumber, int analogChannel)
        {
            if (_firmataStream == null)
            {
                throw new ObjectDisposedException(nameof(FirmataDevice));
            }

            lock (_synchronisationLock)
            {
                _logger.LogInformation($"Disabling analog reporting on pin {pinNumber}, Channel A{analogChannel}");
                if (analogChannel <= 15)
                {
                    _firmataStream.WriteByte((byte)((int)FirmataCommand.REPORT_ANALOG_PIN + analogChannel));
                    _firmataStream.WriteByte((byte)0);
                }
                else
                {
                    FirmataCommandSequence pwmCommandSequence = new();
                    pwmCommandSequence.WriteByte((byte)FirmataSysexCommand.EXTENDED_REPORT_ANALOG);
                    pwmCommandSequence.WriteByte((byte)analogChannel);
                    pwmCommandSequence.WriteByte((byte)0);
                    pwmCommandSequence.WriteByte((byte)FirmataCommand.END_SYSEX);
                    SendCommand(pwmCommandSequence);
        }
            }
        }

        /// <summary>
        /// Check support for System variables. Should be done on init/reinit.
        /// </summary>
        /// <returns>Null if everything is ok, an error message otherwise.</returns>
        internal string? CheckSystemVariablesSupported()
        {
            if (_actualFirmataProtocolVersion.Major == 0)
            {
                // This should not happen
                _systemVariablesSupported = false;
                return "Cannot query System Variables before the firmata version is known";
            }

            if (_actualFirmataProtocolVersion < new Version(2, 7))
            {
                _systemVariablesSupported = false;
                return "Firmata protocol version 2.7 or above required";
            }

            int value = 0;
            try
            {
                // now assume true (otherwise, the method would also fail immediately)
                _systemVariablesSupported = true;
                if (!GetOrSetSystemVariable(SystemVariable.FunctionSupportCheck, -1, true, ref value))
                {
                    _systemVariablesSupported = false;
                    return "System Variable support check failed";
                }
            }
            catch (Exception x) when (x is TimeoutException || x is IOException || x is NotSupportedException)
            {
                _systemVariablesSupported = false;
                return $"GetSystemVariable(FunctionSupportCheck) returned an exception: {x.Message}";
            }

            if (value != 1)
            {
                _systemVariablesSupported = false;
                return "System variables not supported";
            }

            _systemVariablesSupported = true;

            return null;
        }

        public bool GetOrSetSystemVariable(SystemVariable variableId, int pinNumber, bool readValue, ref int value)
        {
            if (!_systemVariablesSupported)
            {
                value = 0;
                return false;
            }

            FirmataCommandSequence cmd = new FirmataCommandSequence();
            cmd.WriteByte((byte)FirmataSysexCommand.SYSTEM_VARIABLE);
            cmd.WriteByte((byte)(readValue ? 0 : 1)); // Query or set
            cmd.WriteByte(1); // Data type: Integer
            cmd.WriteByte(0); // Status (irrelevant)
            cmd.SendInt14((int)variableId);
            if (pinNumber < 127 && pinNumber >= 0)
            {
                cmd.WriteByte((byte)pinNumber);
            }
            else
            {
                cmd.WriteByte(127);
            }

            if (readValue)
            {
                // Don't send garbage in case of a read request.
                value = 0;
            }

            cmd.SendInt32(value);
            cmd.WriteByte((byte)FirmataCommand.END_SYSEX);

            byte[] reply = SendCommandAndWait(cmd, DefaultReplyTimeout, (sequence, bytes) =>
            {
                if (bytes.Length < 12)
                {
                    return false;
                }

                if (bytes[0] != (byte)FirmataSysexCommand.SYSTEM_VARIABLE)
                {
                    return false;
                }

                // Reply must be for the same pin and the same variable Id.
                int replyPin = bytes[6];
                if (replyPin == 127)
                {
                    replyPin = -1;
                }

                if (replyPin != pinNumber)
                {
                    return false;
                }

                int id = FirmataCommandSequence.DecodeInt14(bytes, 4);
                if (id != (int)variableId)
                {
                    return false;
                }

                return true;
            }, out var error);

            _lastCommandError = error;

            if (error != CommandError.None)
            {
                throw new IOException($"Unable to query {variableId}. Command returned status {error}");
            }

            SystemVariableError status = (SystemVariableError)reply[3];
            if (!CheckVariableReplyStatus(variableId, status))
            {
                value = 0;
                return false;
            }

            int replyType = reply[2];
            if (replyType != 1)
            {
                // The firmware indicates that the type of the value is something else than int. This is a problem for now.
                throw new NotSupportedException($"Firmware reports an unknown data type: {replyType}");
            }

            value = FirmataCommandSequence.DecodeInt32(reply, 7);
            return true;
        }

        private bool CheckVariableReplyStatus(SystemVariable variableId, SystemVariableError status)
        {
            switch (status)
            {
                case SystemVariableError.FieldReadOnly:
                    _logger.LogError($"Field {variableId} is read-only");
                    return false;
                case SystemVariableError.FieldWriteOnly:
                    _logger.LogError($"Field {variableId} is write-only");
                    return false;
                case SystemVariableError.GenericError:
                    _logger.LogError($"There was an error processing the request");
                    return false;
                case SystemVariableError.UnknownDataType:
                    _logger.LogError($"The data type is not supported");
                    return false;
                case SystemVariableError.UnknownVariableId:
                    _logger.LogError($"The variable id {variableId} is not supported");
                    return false;
            }

            return true;
        }

        public void EnableSpi()
        {
            FirmataCommandSequence enableSpi = new();
            enableSpi.WriteByte((byte)FirmataSysexCommand.SPI_DATA);
            enableSpi.WriteByte((byte)FirmataSpiCommand.SPI_BEGIN);
            enableSpi.WriteByte((byte)0);
            enableSpi.WriteByte((byte)FirmataCommand.END_SYSEX);
            SendCommand(enableSpi);
        }

        public void DisableSpi()
        {
            FirmataCommandSequence disableSpi = new();
            disableSpi.WriteByte((byte)FirmataSysexCommand.SPI_DATA);
            disableSpi.WriteByte((byte)FirmataSpiCommand.SPI_END);
            disableSpi.WriteByte((byte)0);
            disableSpi.WriteByte((byte)FirmataCommand.END_SYSEX);
            SendCommand(disableSpi);
        }

        public void SpiWrite(int csPin, ReadOnlySpan<byte> writeBytes, bool waitForReply)
        {
            // When the command is SPI_WRITE, the device answer is already discarded in the firmware.
            if (waitForReply)
            {
                FirmataCommandSequence command = SpiWrite(csPin, FirmataSpiCommand.SPI_WRITE_ACK, writeBytes, out byte requestId);
                byte[] response = SendCommandAndWait(command, DefaultReplyTimeout, (sequence, bytes) =>
                {
                    if (bytes.Length < 5)
                    {
                        return false;
                    }

                    if (bytes[0] != (byte)FirmataSysexCommand.SPI_DATA || bytes[1] != (byte)FirmataSpiCommand.SPI_REPLY)
                    {
                        return false;
                    }

                    if (bytes[3] != (byte)requestId)
                    {
                        return false;
                    }

                    return true;
                }, out _lastCommandError);

                if (response[0] != (byte)FirmataSysexCommand.SPI_DATA || response[1] != (byte)FirmataSpiCommand.SPI_REPLY)
                {
                    throw new IOException("Firmata protocol error: received incorrect query response");
                }

                if (response[3] != (byte)requestId)
                {
                    throw new IOException($"Firmata protocol sequence error.");
                }
            }
            else
            {
                FirmataCommandSequence command = SpiWrite(csPin, FirmataSpiCommand.SPI_WRITE, writeBytes, out _);
                SendCommand(command);
            }
        }

        public void SpiTransfer(int csPin, ReadOnlySpan<byte> writeBytes, Span<byte> readBytes)
        {
            FirmataCommandSequence command = SpiWrite(csPin, FirmataSpiCommand.SPI_TRANSFER, writeBytes, out byte requestId);
            byte[] response = SendCommandAndWait(command, DefaultReplyTimeout, (sequence, bytes) =>
            {
                if (bytes.Length < 5)
                {
                    return false;
                }

                if (bytes[0] != (byte)FirmataSysexCommand.SPI_DATA || bytes[1] != (byte)FirmataSpiCommand.SPI_REPLY)
                {
                    return false;
                }

                if (bytes[3] != (byte)requestId)
                {
                    return false;
                }

                return true;
            }, out _lastCommandError);

            if (response[0] != (byte)FirmataSysexCommand.SPI_DATA || response[1] != (byte)FirmataSpiCommand.SPI_REPLY)
            {
                throw new IOException("Firmata protocol error: received incorrect query response");
            }

            if (response[3] != (byte)requestId)
            {
                throw new IOException($"Firmata protocol sequence error.");
            }

            ReassembleByteString(response, 5, response[4] * 2, readBytes);
        }

        private FirmataCommandSequence SpiWrite(int csPin, FirmataSpiCommand command, ReadOnlySpan<byte> writeBytes, out byte requestId)
        {
            requestId = (byte)(_lastRequestId++ & 0x7F);

            FirmataCommandSequence spiCommand = new();
            spiCommand.WriteByte((byte)FirmataSysexCommand.SPI_DATA);
            spiCommand.WriteByte((byte)command);
            spiCommand.WriteByte((byte)(csPin << 3)); // Device ID / channel
            spiCommand.WriteByte(requestId);
            spiCommand.WriteByte(1); // Deselect CS after transfer (yes)
            spiCommand.WriteByte((byte)writeBytes.Length);
            spiCommand.Write(Encoder7Bit.Encode(writeBytes));
            spiCommand.WriteByte((byte)FirmataCommand.END_SYSEX);
            return spiCommand;
        }

        public void SetAnalogInputSamplingInterval(TimeSpan interval)
        {
            int millis = (int)interval.TotalMilliseconds;
            FirmataCommandSequence seq = new();
            seq.WriteByte((byte)FirmataSysexCommand.SAMPLING_INTERVAL);
            int value = millis;
            seq.WriteByte((byte)(value & (uint)sbyte.MaxValue)); // lower 7 bits
            seq.WriteByte((byte)(value >> 7 & sbyte.MaxValue)); // top bits
            seq.WriteByte((byte)FirmataCommand.END_SYSEX);
            SendCommand(seq);
        }

        public void ConfigureSpiDevice(SpiConnectionSettings connectionSettings)
        {
            if (connectionSettings.ChipSelectLine >= 15)
            {
                // this limit is currently required because we derive the device id from the CS line, and that one has only 4 bits
                throw new NotSupportedException("Only pins <=15 are allowed as CS line");
            }

            if (_firmwareVersion <= new Version(2, 11))
            {
                // we could leverage this, if needed, by using the older data encoding
                throw new NotSupportedException("This library requires firmware version 2.12 or later for SPI transfers");
            }

            int deviceId = connectionSettings.ChipSelectLine;
            FirmataCommandSequence spiConfigSequence = new();
            spiConfigSequence.WriteByte((byte)FirmataSysexCommand.SPI_DATA);
            spiConfigSequence.WriteByte((byte)FirmataSpiCommand.SPI_DEVICE_CONFIG);
            byte deviceIdChannel = (byte)(deviceId << 3 | (connectionSettings.BusId & 0x7));
            spiConfigSequence.WriteByte((byte)(deviceIdChannel));
            int dataMode = 0;
            if (connectionSettings.DataFlow == DataFlow.MsbFirst)
            {
                dataMode = 1;
            }

            int mode = ((int)connectionSettings.Mode) << 1;
            dataMode |= mode;
            dataMode |= 0x8; // Use fast transfer mode

            spiConfigSequence.WriteByte((byte)dataMode);
            int clockSpeed = connectionSettings.ClockFrequency;
            if (clockSpeed <= 0)
            {
                clockSpeed = 1_000_000;
            }

            spiConfigSequence.SendInt32(clockSpeed);
            spiConfigSequence.WriteByte(0); // Word size (default = 8)
            spiConfigSequence.WriteByte(1); // Default CS pin control (enable)
            spiConfigSequence.WriteByte((byte)(connectionSettings.ChipSelectLine));
            spiConfigSequence.WriteByte((byte)FirmataCommand.END_SYSEX);
            SendCommand(spiConfigSequence);
        }

        internal uint GetAnalogRawValue(int pinNumber)
        {
            lock (_lastAnalogValueLock)
            {
                return _lastAnalogValues[pinNumber];
            }
        }

        /// <summary>
        /// Firmata uses 2 bytes to encode 8-bit data, because byte values with the top bit set
        /// are reserved for commands. This decodes such data chunks.
        /// </summary>
        private int ReassembleByteString(IList<byte> byteStream, int startIndex, int length, Span<byte> reply)
        {
            int num;
            if (reply.Length < length / 2)
            {
                length = reply.Length * 2;
            }

            for (num = 0; num < length / 2; ++num)
            {
                reply[num] = (byte)(byteStream[startIndex + (num * 2)] |
                                    byteStream[startIndex + (num * 2) + 1] << 7);
            }

            return length / 2;
        }

        private void Dispose(bool disposing)
        {
            if (disposing)
            {
                InputThreadShouldExit = true;

                lock (_synchronisationLock)
                {
                    if (_firmataStream != null)
                    {
                        _firmataStream.Close();
                    }

                    _firmataStream = null;
                }

                StopThread();

                if (_dataReceived != null)
                {
                    _dataReceived.Dispose();
                    _dataReceived = null!;
                }

                OnError = null;
            }
        }

        public void Dispose()
        {
            Dispose(true);
            GC.SuppressFinalize(this);
        }

        public void SendSoftwareReset()
        {
            lock (_synchronisationLock)
            {
                _firmataStream?.WriteByte(0xFF);
            }
        }
    }
}