public UavDataparser(UAVObjectManager dataManager)
 {
     rxTmpBuffer = new ByteBuffer(4);
     rxBuffer = new ByteBuffer(uavConsts.MAX_PAYLOAD_LENGTH);
     _stats = new ComStats();
     rxState = commState.SYNC;
     rxPacketLength = 0;
     _objMgr = dataManager;
 }
        public void processInputByte(byte data)
        {
            _stats.Bytes++;

            rxPacketLength++; // update packet byte count

            switch (rxState)
            {
                case commState.SYNC:
                    if (data != uavConsts.SYNC_VAL)
                    {
                        if (preBufferIdx < prebuffer.Length)
                        {
                            prebuffer[preBufferIdx++] = data;
                        }
                        break;
                    }

                    // Initialize and update CRC
                    rxCS = CRC.updateCRC(0, data);
                    rxPacketLength = 1;
                    rxState = commState.TYPE;
                    break;
                case commState.TYPE:
                    // Update CRC
                    rxCS = CRC.updateCRC(rxCS, data);

                    if ((data & uavConsts.TYPE_MASK) != uavConsts.TYPE_VER)
                    {
                        if (preBufferIdx + 1 < prebuffer.Length)
                        {
                            prebuffer[preBufferIdx++] = uavConsts.SYNC_VAL;
                            prebuffer[preBufferIdx++] = data;
                        }
                        //Debug.WriteLine( "Unknown UAVTalk type: {0:X}", data);
                        rxState = commState.SYNC;
                        break;
                    }

                    if (preBufferIdx == prebuffer.Length)
                    {
                        last_timestamp = BitConverter.ToUInt32(prebuffer, 0);
                        UInt64 packetSize_pre = BitConverter.ToUInt64(prebuffer, 4);
                    }
                    preBufferIdx = 0;
                    rxType = data;
                    //Debug.WriteLine("Received packet type:  {0:X}", data);
                    packetSize = 0;

                    rxState = commState.SIZE;
                    rxCount = 0;
                    break;
                case commState.SIZE:
                    // Update CRC
                    rxCS = CRC.updateCRC(rxCS, data);

                    if (rxCount == 0)
                    {
                        packetSize += data;
                        rxCount++;
                        break;
                    }

                    packetSize += (data << 8) & 0xff00;

                    if (packetSize < uavConsts.MIN_HEADER_LENGTH
                            || packetSize > uavConsts.MAX_HEADER_LENGTH + uavConsts.MAX_PAYLOAD_LENGTH)
                    { // incorrect
                        // packet
                        // size
                        rxState = commState.SYNC;
                        break;
                    }

                    rxCount = 0;
                    rxState = commState.OBJID;
                    rxTmpBuffer.position(0);
                    break;
                case commState.OBJID:
                    // Update CRC
                    rxCS = CRC.updateCRC(rxCS, data);

                    rxTmpBuffer.put(rxCount++, data);
                    if (rxCount < 4)
                        break;

                    // Search for object, if not found reset state machine
                    rxObjId = rxTmpBuffer.getInt(0);
                    // Because java treats ints as only signed we need to do this manually

                    UAVObject rxObj = _objMgr.getObject(rxObjId);
                    if (rxObj == null)
                    {
                        Debug.WriteLine("Unknown ID: " + rxObjId);
                        _stats.Errors++;
                        rxState = commState.SYNC;
                        break;
                    }

                    // Determine data length
                    if (rxType == uavConsts.TYPE_OBJ_REQ || rxType == uavConsts.TYPE_ACK || rxType == uavConsts.TYPE_NACK)
                        rxLength = 0;
                    else
                        rxLength = rxObj.getNumBytes();

                    // Check length and determine next state
                    if (rxLength >= uavConsts.MAX_PAYLOAD_LENGTH)
                    {
                        Debug.WriteLine("Greater than max payload length");
                        _stats.Errors++;
                        rxState = commState.SYNC;
                        break;
                    }

                    // Check if this is a single instance object (i.e. if the
                    // instance ID field is coming next)
                    if (rxObj.isSingleInstance())
                    {
                        // If there is a payload get it, otherwise receive checksum
                        if (rxLength > 0)
                            rxState = commState.DATA;
                        else
                            rxState = commState.CS;
                        rxInstId = 0;
                        rxCount = 0;
                    }
                    else
                    {
                        rxState = commState.INSTID;
                        rxCount = 0;
                    }

                    break;
                case commState.INSTID:

                    // Update CRC
                    rxCS = CRC.updateCRC(rxCS, data);

                    rxTmpBuffer.put(rxCount++, data);
                    if (rxCount < 2)
                        break;

                    rxInstId = rxTmpBuffer.getShort(0);

                    rxCount = 0;

                    // If there is a payload get it, otherwise receive checksum
                    if (rxLength > 0)
                        rxState = commState.DATA;
                    else
                        rxState = commState.CS;

                    break;
                case commState.DATA:

                    // Update CRC
                    rxCS = CRC.updateCRC(rxCS, data);

                    rxBuffer.put(rxCount++, data);
                    if (rxCount < rxLength)
                        break;

                    rxState = commState.CS;
                    rxCount = 0;
                    break;
                case commState.CS:

                    // The CRC byte
                    rxCSPacket = data;

                    if (rxCS != rxCSPacket)
                    { // packet error - faulty CRC
                        Debug.WriteLine("Bad crc");
                        _stats.Errors++;
                        rxState = commState.SYNC;
                        break;
                    }

                    if (rxPacketLength != (packetSize + 1))
                    { // packet error -
                        // mismatched packet
                        // size
                        Debug.WriteLine("Bad size");
                        _stats.Errors++;
                        rxState = commState.SYNC;
                        break;
                    }

                    rxBuffer.position(0);
                    if(onObjectReceived != null)
                        onObjectReceived(rxType, rxObjId, rxInstId, last_timestamp, rxBuffer);
                    _stats.ObjectBytes += rxLength;
                    _stats.Objects++;

                    rxState = commState.SYNC;
                    break;
                default:
                    Debug.WriteLine("Bad state");
                    rxState = commState.SYNC;
                    _stats.Errors++;
                    break;
            }
        }