context: //-----------------------------------------------------------------------
  //  This file is part of Microsoft Robotics Developer Studio Code Samples.
  //
  //  Copyright (C) Microsoft Corporation.  All rights reserved.
  //
  //  $File: Packet.cs $ $Revision: 7 $
  //-----------------------------------------------------------------------
  
  // This file was modfied to be used in VVVV plugins
  
  using System;
  using System.Collections.Generic;
  using System.Text;
  using System.IO.Ports;
  using System.Diagnostics;
  using VVVV.Core.Logging;
  
  namespace Muthesius.SickLRF
  {
      internal class Packet
      {
          public byte[] _data;
  		public DateTime TimeStamp;
          #region Constructors
  
          public Packet(List<byte> data)
          {
              _data = data.ToArray();
          }
  
          Packet(byte stx, byte address, byte command)
          {
              _data = new byte[7];
  
              _data[0] = stx;
              _data[1] = address;
              Write(2, 1);
              _data[4] = command;
              Write(5, CalculatedChecksum);
          }
  
          Packet(byte stx, byte address, byte[] command)
          {
              _data = new byte[6 + command.Length];
              _data[0] = stx;
              _data[1] = address;
              Write(2, (ushort)command.Length);
              command.CopyTo(_data, 4);
              Write(4 + command.Length, CalculatedChecksum);
          }
  
          #endregion
  
          #region Component accessors
          public byte STX
          {
              get { return _data[0]; }
          }
          public byte Address
          {
              get { return _data[1]; }
          }
          public ushort Length
          {
              get { return ReadUshort(2); }
          }
          public byte Command
          {
              get { return _data[4]; }
          }
          public byte Response
          {
              get { return _data[4]; }
          }
          public byte[] Data
          {
              get
              {
                  byte[] data = new byte[Length];
  
                  Array.Copy(_data, 4, data, 0, Length);
  
                  return data;
              }
          }
      	public byte PacketStatus
      	{
      		get { return GoodChecksum ? _data[Length+1] : (byte)0x00; }
      	}
      	public bool IsDirty {
      		get { return ((PacketStatus>>8) & 0x01) > 0; }
      	}
      	public bool FaultyMeasurement {
      		get { return ((PacketStatus>>7) & 0x01) > 0; }
      	}
          public ushort Checksum
          {
              get { return ReadUshort(4 + Length); }
          }
          public ushort CalculatedChecksum
          {
              get { return CreateCRC(_data, 0, 4 + Length); }
          }
  
          public bool GoodChecksum
          {
              get
              {
                  return CalculatedChecksum == Checksum;
              }
          }
          #endregion
  
          #region static Constructors
  
          public static Packet InitializeAndReset
          {
              get
              {
                  return new Packet(0x02, 0x00, 0x10);
              }
          }
  
          public static Packet MonitoringMode(byte mode)
          {
              return new Packet(0x02, 0x00, new byte[] { 0x20, mode });
          }
  
          public static Packet RequestMeasured(byte mode)
          {
              return new Packet(0x02, 0x00, new byte[] {0x30, mode});
          }
  
          public static Packet Status
          {
              get
              {
                  return new Packet(0x02, 0x00, 0x31);
              }
          }
  
          #endregion
  
          #region IO
          public void Send(SerialPort port)
          {
              port.Write(_data, 0, _data.Length);
          }
  
          public static Packet Read(SerialPort port)
          {
              try
              {
                  List<byte> data = new List<byte>();
  
                  byte b = 0;
  
                  while (b != 0x02)
                  {
                      if (port.BytesToRead == 0)
                      {
                          return null;
                      }
                      b = (byte)port.ReadByte();
                  }
                  // STX
                  data.Add(b);
                  // Address
                  data.Add((byte)port.ReadByte());
                  // Low Length
                  data.Add((byte)port.ReadByte());
                  // High Length
                  data.Add((byte)port.ReadByte());
  
                  ushort length = MakeUshort(data[2], data[3]);
  
                  for (int i = 0; i < length; i++)
                  {
                      data.Add((byte)port.ReadByte());
                  }
  
                  // Low Checksum
                  data.Add((byte)port.ReadByte());
                  // High Checksum
                  data.Add((byte)port.ReadByte());
  
                  return new Packet(data);
              }
              catch
              {
                  return null;
              }
          }
  
          #endregion
  
          #region Utility Functions
  
          public ushort ReadUshort(int index)
          {
              return (ushort)(_data[index] | (_data[index + 1] << 8));
          }
  
          void Write(int index, ushort value)
          {
              _data[index] = (byte)(value & 0xFF);
              _data[index + 1] = (byte)((value >> 8) & 0xFF);
          }
  
          public static ushort MakeUshort(byte LowByte, byte HighByte)
          {
              return (ushort)(LowByte | (HighByte << 8));
          }
          #endregion
  
          #region CRC Function
          const ushort CRC16_GEN_POL = 0x8005;
          static ushort CreateCRC(byte[] CommData, int start, int end)
          {
              ushort crc;
              byte low, high;
  
              crc = 0;
              low = 0;
  
              for (int index = start; index < end; index++)
              {
                  high = low;
                  low = CommData[index];
  
                  if ((crc & 0x8000) == 0x8000)
                  {
                      crc = (ushort)((crc & 0x7FFF) << 1);
                      crc ^= CRC16_GEN_POL;
                  }
                  else
                  {
                      crc <<= 1;
                  }
  
gt:                 crc ^= MakeUshort(low, high);