private void spTeensy_DataReceived(object sender, SerialDataReceivedEventArgs e)
{
while (spTeensy.BytesToRead > 0)
{
byte b = (byte)spTeensy.ReadByte();
Invoke(new delVoidIntBool(AppendByteToSerialLog), b, true); // true = inbound
if (_currentCommand == null)
{
if (_currentSequence == null)
{
if (b == _magicSequence[0])
{
_currentSequence = _magicSequence;
}
else
{
_currentSequence = _respMagicSequence;
}
}
if (_magicSequenceIndex < _currentSequence.Length)
{
if (b != _currentSequence[_magicSequenceIndex])
{
continue; // Ignore byte: Unexpected
}
_magicSequenceIndex++;
continue; // Don't handle the byte
}
Console.WriteLine("Magic value read successfully");
_currentCommand = CommandRegistry.FindCommand(b);
if (_currentCommand == null)
{
Console.WriteLine("Failed to find command for ID: " + b + ", ignoring byte.");
continue;
}
}
var result = _currentCommand.ConsumeByte(b);
if (result == ConsumeResult.InvalidByte)
{
Console.WriteLine("Command " + _currentCommand.CommandId + " did not accept byte: " + b + ", ignoring byte.");
}
else if (result == ConsumeResult.NotConsumed)
{
Console.WriteLine("Command " + _currentCommand.CommandId + " did not consume byte: " + b + ", ignoring byte.");
}
if (!_currentCommand.ExpectingMoreBytes)
{
Invoke(new delVoidBool(NewLine), true); // true = inbound
if (_currentSequence != _respMagicSequence)
{
Invoke(new delVoidCommand(HandleCommand), _currentCommand);
}
_currentCommand = null;
_currentSequence = null;
_magicSequenceIndex = 0;
}
}
}