public void PackageFound(Package package) { if (package.IsAck) { if (_waitingForAcknowledment.ContainsKey(package.CommandID)) { Command cmd = _waitingForAcknowledment[package.CommandID].Command; if (Protocol.ArraysAreEqual(cmd.ToByteData(), package.Payload)) { // We have received acknowledgement, remove from waiting dictionary Console.WriteLine("Command {0} was acknowledged!", package.CommandID); Acknowledgement ack = _waitingForAcknowledment[package.CommandID]; _waitingForAcknowledment.Remove(package.CommandID); _listener?.OnAcknowledge(ack); } else { Console.WriteLine("Acknowledgement for command {0} did not have the correct data", package.CommandID); return; } } else { Console.WriteLine("An non-existing command was acknowledged: {0}", package.CommandID); } } else { DataPacket packet = new DataPacket(package.Payload); _dataLog.Enqueue(packet); } }