/// <summary>
/// Checks a reply for error. If reply has error relevant exception is thrown
/// </summary>
/// <param name='reply'>
/// The reply to check
/// </param>
/// <param name='expectedLength'>
/// Expected reply length
/// </param>
/// <param name='expectedSequenceNumber'>
/// Expected sequence number
/// </param>
/// <param name='cleanUp'>
/// Clean up method called before the exception is thrown
/// </param>
public static void CheckForError(Reply reply, int expectedLength, UInt16 expectedSequenceNumber, CleanUpMethod cleanUp){
CheckForError (reply, expectedLength, expectedSequenceNumber, cleanUp, false);
}
private static void CheckForError(Reply reply, int expectedLength, UInt16 expectedSequenceNumber, CleanUpMethod cleanUp, bool ignoreLength){
if(reply.HasError){
if(cleanUp!= null){
cleanUp();
}
ThrowException(reply);
}
if (!ignoreLength) {
if (reply.Length != expectedLength) {
if (cleanUp != null) {
cleanUp ();
}
throw new BrickException (BrickError.WrongNumberOfBytes);
}
}
if(reply.SequenceNumber != expectedSequenceNumber){
if(cleanUp!= null){
cleanUp();
}
throw new BrickException(BrickError.WrongSequenceNumber);
}
}
/// <summary>
/// Checks a reply for error. If reply has error relevant exception is thrown
/// </summary>
/// <param name='reply'>
/// The reply to check
/// </param>
/// <param name='expectedLength'>
/// Expected reply length
/// </param>
/// <param name='expectedSequenceNumber'>
/// Expected sequence number
/// </param>
public static void CheckForError(Reply reply, int expectedLength, UInt16 expectedSequenceNumber){
CheckForError(reply,expectedLength,expectedSequenceNumber,null, false);
}
/// <summary>
/// Checks a reply for error. If reply has error relevant exception is thrown
/// </summary>
/// <param name='reply'>
/// The reply to check
/// </param>
/// <param name="expectedSequenceNumber">Expected sequence number.</param>
public static void CheckForError(Reply reply, UInt16 expectedSequenceNumber){
CheckForError(reply,0,expectedSequenceNumber,null,true);
}
/// <summary>
/// Throws a monobrick exception based on the Reply from the brick
/// </summary>
/// <param name='reply'>
/// Reply to base the exception on
/// </param>
public static void ThrowException(Reply reply){
ThrowException(reply.ErrorCode, reply.ErrorType);
}
private void ClientThread(object stateInfo)
{
byte[] message = null;
byte[] lengthBytes = new byte[2];
int bytesToRead;
int bytesRead;
//Stopwatch stopWatch = new Stopwatch();
//Stopwatch stopWatch2 = new Stopwatch();
NXT.Command nxtCommand = null;
NXT.Reply nxtReply = null;
EV3.Command ev3Command = null;
EV3.Reply ev3Reply = null;
logQueue.AddToQueue(IdName + "Connect from " + address);
IsConnected = true;
bool run = true;
while (run)
{
bytesToRead = 0;
bytesRead = 0;
message = null;
try
{
bytesRead = networkStream.ReadAll(lengthBytes);
if (bytesRead > 0)
{
bytesToRead = (ushort)(0x0000 | lengthBytes[0] | (lengthBytes[1] << 2));
message = new byte[bytesToRead];
bytesRead = 0;
bytesRead = networkStream.ReadAll(message);
bool CommandValid = true;
if (bytesRead == bytesToRead)
{
try{
if (type == BrickType.NXT)
{
nxtCommand = new NXT.Command(message);
}
else
{
ev3Command = new EV3.Command(message);
}
}
catch (Exception) {
CommandValid = false;
logQueue.AddToQueue(IdName + "Invalid command is ignored");
}
}
else
{
CommandValid = false;
run = false;
logQueue.AddToQueue(IdName + "Not enough bytes read");
}
if (CommandValid)
{
nxtReply = null;
mutex.WaitOne();
try{
if (type == BrickType.NXT)
{
if ((nxtCommand).CommandType == NXT.CommandType.TunnelCommand)
{
if (TunnelCommandReceived == null)
{
nxtReply = new NXT.Reply(NXT.CommandType.ReplyCommand, nxtCommand.CommandByte, (byte)TunnelError.UnsupportedCommand);
}
else
{
var brickReply = TunnelCommandReceived(nxtCommand);
nxtReply = new NXT.Reply(brickReply.Data);
}
SendNetworkReply(nxtReply);
}
else
{
if (nxtCommand.ReplyRequired)
{
if (LogActivity)
{
logQueue.AddToQueue(IdName + "Forward ".PadRight(12) + nxtCommand.CommandByte.ToString() + " with reply to NXT");
}
}
else
{
if (LogActivity)
{
logQueue.AddToQueue(IdName + "Forward ".PadRight(12) + nxtCommand.CommandByte.ToString() + " without reply to NXT");
}
}
Brick.Send(nxtCommand);
if (nxtCommand.ReplyRequired)
{
nxtReply = (NXT.Reply)Brick.Receive();
SendNetworkReply(nxtReply);
if (LogActivity)
{
logQueue.AddToQueue(IdName + "Received ".PadRight(12) + nxtReply.CommandByte.ToString() + " from NXT");
}
}
}
}
else
{
throw new NotImplementedException("EV3 support is not implemented");
}
}
catch (Exception e) {
if (e is MonoBrickException)
{
if (e is ConnectionException)
{
logQueue.AddToQueue(IdName + e.Message);
logQueue.AddToQueue("Closing connection to client " + ID);
run = false;
}
else
{
logQueue.AddToQueue(IdName + e.Message);
if (nxtReply == null) //try to send the message with error
{
nxtReply = new NXT.Reply(NXT.CommandType.ReplyCommand, nxtCommand.CommandByte, ((MonoBrickException)e).ErrorCode);
SendNetworkReply(nxtReply);
}
if (ev3Reply == null)
{
throw new NotImplementedException("EV3 support is not implemented");
}
}
}
else
{
if (!wasThrownOff)
{
logQueue.AddToQueue(IdName + e.Message);
logQueue.AddToQueue(IdName + "--------- Stack Trace --------\n" + e.StackTrace);
}
run = false;
}
}
mutex.ReleaseMutex();
}
}
else
{
run = false;
}
}
catch (Exception e)
{
if (!wasThrownOff)
{
logQueue.AddToQueue(IdName + e.Message);
logQueue.AddToQueue(IdName + "--------- Stack Trace --------\n" + e.StackTrace);
}
break;
}
}
try{
networkStream.Close();
}
catch {}
logQueue.AddToQueue(IdName + "Disconnected");
if (onDisconnected != null)
{
onDisconnected(this);
}
IsConnected = false;
}
public void MessageReceived(Reply reply)
{
if (OnMessageReceived != null)
{
if (!ignore)
{
OnMessageReceived(this, new MessageEventArgs(System.Text.Encoding.UTF8.GetString(reply.Data)));
}
ignore = !ignore;
}
}
