/* ********************************************************************************
 * Arduino_BT_Slave.pde
 * Author     : CH, Chen (Taiwan)
 * Date       : 2012.5.27
 *
 * Data Packetage for Bluetooth Communication:(P22 of NXT Communication Protocol)
 * Length LSB + Length MSB + Command Type + Command + Data bytes...
 *
 * Data Payload of MESSAGEWRITE Direct Command
 * (Received from NXT)
 * Byte 0: Command type(0x00 or 0x80)
 * Byte 1: Direct Command-MESSAGEWRITE(0x09)
 * Byte 2: Inbox number(0 - 9)
 * Byte 3: Message size, include null termination byte
 * Byte 4 - N: Message data, where N = Message size + 3,
               is treated as a string, must include null termination.
 * (Return package)
 * Byte 0: 0x02
 * Byte 1: 0x09
 * Byte 2: Status Byte
 * ******************************************************************************** */
#define MIN_PACKET_SIZE   7
#define MAX_PACKET_SIZE  64

byte rcvMessage [MAX_PACKET_SIZE];
byte rcvByte;
int  msgIndex=0, packetLength=0;
char strMsg [256];

void printMessage()
{  
  // Value of packetLength is the total length of data packet that will be received, 
  // but not include the first two length bytes. 
  int packetLength = word(rcvMessage[1], rcvMessage[0]);
  if(packetLength == (msgIndex-2))
  {
    sprintf(strMsg, "Packet size: %d bytes", packetLength);
    Serial.println(strMsg);
    
    sprintf(strMsg, "Command Type: %X, Command: %X, InBox: %d ", rcvMessage[2], rcvMessage[3], rcvMessage[4]);
    Serial.println(strMsg);
    
    Serial.println("Received Message: ");    
    for(int xii=6; xii<(packetLength+2); xii++) Serial.print((char)rcvMessage[xii]);
            
    sprintf(strMsg, "   (Total %d bytes)", rcvMessage[5]);
    Serial.print(strMsg);
    
    Serial.println("\n\r");
  }
}

void setup()
{
  Serial.begin(9600);
}

void loop()
{
  while(Serial.available() > 0)
  {    
    rcvMessage[msgIndex] = Serial.read();
    msgIndex++;
    if(msgIndex > MAX_PACKET_SIZE)
    {
      Serial.flush();
      break;
    }
  }
  
  if(msgIndex > MIN_PACKET_SIZE)
  {          
    printMessage();
    msgIndex = 0;
  }
  delay(300);
}