Ping Pong Packets

After another week of work on my packet code, I finally have a working example of call and response working. Processing sends a Ping packet to the Arduino and the Arduino replies back Pong along with the payload of the original packet. Next step is to expand this to connect to wireless nodes.

Code after the jump


Processing:

import processing.serial.*;

//Device Codes
final int DVC_LM33          = 0x01;
final int DVC_ST11          = 0x02;
final int DVC_ST15          = 0x03;
final int DVC_DOORCONTACT   = 0x04;
final int DVC_FLIPSWITCH    = 0x05;
final int DVC_LIGHT         = 0x06;
final int DVC_MOTION        = 0x07;
final int DVC_2LINECHAR     = 0x08;
final int DVC_POWER30       = 0x08;
final int DVC_POWER100      = 0x09;
final int DVC_SMARTOUTLET   = 0x10;
final int DVC_RGBLIGHT      = 0x11;

//Command Codes
final int  CMD_GET_VERSION   = 0x01;
final int  CMD_PING          = 0x3E;
final int  CMD_PONG          = 0xE3;
final int  CMD_ACK           = 0x11;
final int  CMD_READ          = 0xAA;
final int  CMD_WRITE         = 0xBB;
final int  CMD_PORTTYPE      = 0xCC;

final int  CMD_AUTOSENDSTART = 0xC1;
final int  CMD_AUTOSENDSTOP  = 0xC2;

final int  CMD_ERROR         = 0xFF;

//shortcuts to important bytes
//keeps from having to decode the whole thing to check simple parts
final int PACKET_LENGTH   = 0;
final int PACKET_SETTINGS = 1;
final int PACKET_FROM     = 2;
final int PACKET_TO       = 4;
final int PACKET_TTL      = 4;
final int PACKET_ID       = 6;
final int PACKET_COMMAND  = 7;
final int PACKET_PAYLOAD  = 8;

//an enum might be better here
final int STATUS_CLEAR    = 0;
final int STATUS_RECEVING = 1;
final int STATUS_RECEVIED = 2;
final int STATUS_WAITING  = 3;
final int STATUS_READY    = 4;
final int STATUS_SENDING  = 5;
final int STATUS_SENT     = 6;
final int STATUS_ACK      = 7;
final int STATUS_SUCCESS  = 8;

final int OFFSET_PACKET = 10;
final int OFFSET_HEADER = 8;
final int OFFSET_FOOTER = 2;



class DataPacketDecoded {
  int command, length, status = 0, settings, type,  fromPort, toPort, id, ttl = 0, ptr = 0;
  int fromNode, toNode,  checksum = 0;
  int[] payload = new int[56];

  DataPacketDecoded(){ }
}

//simple data packet

class DataPacket {
  int ptr = 0, status = 0, checksum = 0;
  int[] payload = new int[64];
  
  DataPacket(){ }
} 

class Payload {
  int length;
  int[] data = new int[56];
  
  Payload() { }
} 

//What should master ports be?

final int  NODE_ID = 0x02;

final int PORT_A = DVC_LM33;
final int PORT_B = DVC_LM33;
final int PORT_C = DVC_LM33;
final int PORT_D = DVC_LM33;

final int PACKET_BUFFER  = 10;
final int PACKET_TIMEOUT = 100;

DataPacket[] data = new DataPacket[PACKET_BUFFER];
DataPacket incomingPacket = new DataPacket(); //current incoming packet

int checksum;
int uniqueId = 0;
int i;
boolean sending = false;
boolean receiving = false;
long packetTimer;



int bgcolor;			     // Background color
int fgcolor;			     // Fill color
Serial myPort;                       // The serial port
PFont myFont;

//build rf12 packet

void build_incoming_packet(int byteIn)
{
   //mark which port it is coming from
  println("count: "+incomingPacket.ptr + " byte: " + byteIn);
 
  if(incomingPacket.ptr == 0){  //Packet length
    if(byteIn <= 1) {
      println("RESENDING LAST");
      //resend last
      sendingPacket.status = STATUS_SENDING;
      return;
    }
    if(byteIn == 255) {
      println("ACK SUCCESS");
      sendingPacket.status = STATUS_SUCCESS; 
      sending = false; //allow next packet to send
      //aknowledge
      return;
    }
    if (byteIn > 128) {
      myPort.clear();
      myPort.write(0);
      return;
    }
    receiving = true;
    packetTimer = millis();
    incomingPacket.payload[PACKET_LENGTH] = byteIn;
    incomingPacket.status = STATUS_RECEVING;
    checksum = 0;
    
    println("length: " + byteIn);
  }
  
  //checksum 
  if(incomingPacket.ptr < (incomingPacket.payload[PACKET_LENGTH] - OFFSET_FOOTER)){ //add to check sum
    incomingPacket.payload[incomingPacket.ptr] = byteIn;
    checksum = _crc16_update(checksum, byteIn);
    incomingPacket.ptr++;
    return;
  } else if(incomingPacket.ptr == (incomingPacket.payload[PACKET_LENGTH] - OFFSET_FOOTER)){ //get first byte of checksum
    incomingPacket.checksum = byteIn;
    incomingPacket.ptr++;
    return;
  } else { //get second byte and check the checksum
    incomingPacket.checksum = (incomingPacket.checksum << 8) | (byteIn & 0xFF);         
    if(checksum != incomingPacket.checksum) {
      println("processing checksum: " + checksum);  
      println("    packet checksum: " + incomingPacket.checksum);   
      println("Failed CRC Check"); 
      myPort.clear();
      myPort.write(int(0));
    } else {
      println("processing checksum: " + checksum);  
      println("    packet checksum: " + incomingPacket.checksum);  
      println("Passed CRC Check");  
      incomingPacket.status=STATUS_RECEVIED;
      DataPacket packet = getNewPacket();
      packet = incomingPacket;
      
      myPort.clear();
      myPort.write(int(255));
      println("SENT ACK");
      //packet
      debug_incoming_packet(packet); 
      // incomingPacket.payload;
      
      //incomingPacket = DataPacket;
    }
    //reset incoming packet
    incomingPacket = new DataPacket();
    receiving = false;
  }
  
  
}


//determine where to send packet. how to expand when we have 4 serial ports?

void process_stack(){
  //sending = false; //only send one pack per cycle
  
  for(i=0;i//check for packets to send

//check for packets to resend if ACKs are late
   
//check for data packets that can be deleted from the stack 
}

DataPacket sendingPacket;

void process_packet(DataPacket packet){
  switch (packet.status){
    case STATUS_CLEAR:
      //println("CLEAR");
      break;
    case STATUS_RECEVING:
      println("RECEVING");

      break;
    case STATUS_RECEVIED:
      process_command(packet);
      packet.status = STATUS_CLEAR;
      println("RECEVIED");
      break;
    case STATUS_WAITING:
      println("WAITING");
      break;
    case STATUS_READY:
      //check for free port
      if(sending == false){
        packet.status = STATUS_SENDING;
        sending = true;
      }
      //if reset pointer and set to send
      println("READY");
      break;
    case STATUS_SENDING:
      //because of the above, only one packet will be changed to sending
      //if(get_node(packet,PACKET_FROM) < 30){
        // send_packet_rf12(packet); 
      //} else {
         send_packet_serial(packet); 
         sendingPacket = packet;
     // }
      packet.status = STATUS_SENT;
      println("SENDING");
      break;
    case STATUS_SENT:
      //if is a ack packet 
      if(packet.payload[PACKET_SETTINGS] == 1){
        packet.status = STATUS_ACK;
        //start packets ack timer
      } else {
        //packet.status = STATUS_SUCCESS;
      }
      println("SENT");
      break;
    case STATUS_ACK:
       //if execeded timer resen
       //count resends if exceds max kill packet and add error
       println("ACK");
      break;
    case STATUS_SUCCESS:
      //option to post status to screen
      packet.status = STATUS_CLEAR;
      println("SUCCESS");
      break; 
  }
}

long sendTimer;

void setup() {
  size(600, 400);  // Stage size
  //noStroke();      // No border on the next thing drawn
  myFont = createFont("Consolas", 14);
  textFont(myFont);
  
  for(i=0; i < PACKET_BUFFER; i++) {
    data[i] = new DataPacket();    
  }
  

  // Print a list of the serial ports, for debugging purposes:
  //println(PFont.list());
  sendTimer = millis();
  String portName = Serial.list()[1];
  myPort = new Serial(this, portName, 115200);
}

void draw() {
  process_stack();
  //process_stack();
  //process_stack();
 // process_stack();
 // process_stack();
  println("loop");
  //delay(1000);
   //text("Hello",10,10);
   //text("Z", 10, 220);
   
  // Draw the shape
  if((millis() - sendTimer) > 2500){
    DataPacket packet = build_udp_packet(0,0,1,0,CMD_PING,str_to_payload("Hello"));
    packet.status = STATUS_READY;
    sendTimer=millis();
  }

  
  //send_packet_serial(packet);
  if(receiving == true) {
    if((millis() - packetTimer) > PACKET_TIMEOUT) {
      myPort.clear();
      myPort.write(0); //tell the node to resend last
      
      incomingPacket = new DataPacket();
      println("bad packet");
      receiving = false;
    }
  }
  

}

void serialEvent(Serial myPort) {
  // read a byte from the serial port:
  int inByte = myPort.read();
  build_incoming_packet(inByte);
  //print(inByte);
  //print(",");
}

//Done
DataPacket getNewPacket() {
  for(i=0; i < PACKET_BUFFER; i++) {
    if(data[i].status == STATUS_CLEAR){
      
      return data[i];
    } 
  }
  println("BUFFER FULL");
  return new DataPacket();
}

int get_node(DataPacket packet, int location)
{
  int node;  
  node = packet.payload[location];
  node = node << 4;
  node = node | (packet.payload[location+1] >> 4);
  return node;
}

Payload get_payload(DataPacket packet) 
{
  Payload payload = new Payload();
  payload.length = packet.payload[PACKET_LENGTH] - OFFSET_PACKET;
  for(i=0; i < payload.length; i++){
    if(packet.payload[PACKET_SETTINGS] < 3) { //single packet TCP or UDP
      payload.data[i] = packet.payload[i+OFFSET_HEADER];   
     
    } else if(packet.payload[PACKET_SETTINGS] == 3) {  //broadcast
        //todo
    }
  }
  return payload; 
}
 
Payload str_to_payload(String string)
{
  Payload payload = new Payload();
  //println("Payload Length: "+ string.length());
  for(i=0;ilength();i++){
    payload.data[i] = int(string.charAt(i)); 
  }
  payload.length = string.length();
  return payload; 
}

void process_command(DataPacket packet)
{
  switch (packet.payload[PACKET_COMMAND]) {
    case CMD_PING:
      
      DataPacket reply = build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, get_payload(packet));
      reply.status = STATUS_READY;
      
      //change status to ready to send
      
      break;
    //default:
      //send_packet(COMMAND_ERROR,NULL,0);
   //   break;
    }
}

int _crc16_update(int crc, int a)
{
    int i;
    crc ^= a;
    for (i = 0; i < 8; ++i){
        if ((crc & 1) == 1){
            crc = (crc >> 1) ^ 0xA001;
        }else{
            crc = (crc >> 1);
        }
    }
    return crc & 0xFFFF;
}

int getId() {
  if(uniqueId == 255){
    uniqueId = 0;
  }
  return uniqueId++; //what happens when this wraps?
}

int compressAddress(int node, int port) 
{
  node = node << 4; //make space
  port = port & 0x0F;
  return node | port;
}


DataPacket build_tcp_packet(int fromNode, int fromPlug, int toNode, int toPlug, int command, Payload payload)
{
  DataPacket packet = getNewPacket();
  //*packet.status = STATUS_READY;
  packet.status = STATUS_WAITING;
  packet.payload[PACKET_LENGTH] = payload.length + OFFSET_PACKET; //header 8 crc 2
  packet.payload[PACKET_SETTINGS] = 1;
  int address = compressAddress(fromNode, fromPlug);
  packet.payload[PACKET_FROM] = (address >> 8) & 0xFF;
  packet.payload[PACKET_FROM+1] = address & 0xFF;
  address = compressAddress(toNode, toPlug);
  packet.payload[PACKET_TO ] = (address >> 8) & 0xFF;
  packet.payload[PACKET_TO+1] = address & 0xFF;
  packet.payload[PACKET_ID] = getId();
  packet.payload[PACKET_COMMAND] = command;
  
  for(i=0;i < payload.length;i++){
    packet.payload[PACKET_PAYLOAD+i] = payload.data[i];
  }

  return packet;
}

/*
 * @return DataPacket * Pointer to the New DataPacket
 */

DataPacket build_udp_packet(int fromNode, int fromPlug, int toNode, int toPlug, int command, Payload payload)
{
  DataPacket packet = getNewPacket();
  //*packet.status = STATUS_READY;
  packet.status = STATUS_WAITING;
  packet.payload[PACKET_LENGTH] = payload.length + OFFSET_PACKET; //header 8 crc 2
  packet.payload[PACKET_SETTINGS] = 2;
  int address = compressAddress(fromNode, fromPlug);
  packet.payload[PACKET_FROM] = (address >> 8) & 0xFF;
  packet.payload[PACKET_FROM+1] = address & 0xFF;
  address = compressAddress(toNode, toPlug);
  packet.payload[PACKET_TO ] = (address >> 8) & 0xFF;
  packet.payload[PACKET_TO+1] = address & 0xFF;
  packet.payload[PACKET_ID] = getId();
  packet.payload[PACKET_COMMAND] = command;
  
  for(i=0;i < payload.length;i++){
  packet.payload[PACKET_PAYLOAD+i] = payload.data[i];
  }
  
  return packet;
}

/*
uint8_t build_broadcast_packet(uint16_t fromNode, uint8_t fromPort, uint16_t toNode, uint8_t toPort, uint8_t payload[48], uint8_t length, uint8_t ttl )
{
  byte packetId = getNewPacketId();
  data[packetId].sent = 0;
  return packetId;
}
*/

boolean send_packet_serial(DataPacket packet)
{
  int dataChecksum = 0;
  for(i=0; i < (packet.payload[PACKET_LENGTH]-2); i++){
    dataChecksum = _crc16_update(dataChecksum, packet.payload[i]);
    myPort.write(packet.payload[i] & 0xFF);
    print(int((packet.payload[i] & 0xFF)) + ",");
  } 
  
  print((dataChecksum >>8)&0xFF);
  print(",");
  print(dataChecksum & 0xFF);
  
  println("");
  println("Packet Sent");
  packet.status = 1;
  myPort.write((dataChecksum >> 8) & 0xFF);	
  myPort.write(dataChecksum & 0xFF);
  
 
  
  return true;
}

/**
 * turns a packet into a decoded packet
 */

DataPacketDecoded decode_packet(DataPacket packet)
{
  DataPacketDecoded decodedPacket = new DataPacketDecoded();
       
  //Set Packet length
  decodedPacket.length = packet.payload[PACKET_LENGTH];
  
  decodedPacket.status = packet.status;
  
  //settings 
  decodedPacket.settings = packet.payload[PACKET_SETTINGS];
  decodedPacket.type = packet.payload[PACKET_SETTINGS]; //right now these are the same things

  //From 12bit node id, 4 bit plug id
  decodedPacket.fromNode = packet.payload[PACKET_FROM];
  decodedPacket.fromNode = decodedPacket.fromNode << 4;
  decodedPacket.fromNode = decodedPacket.fromNode | (packet.payload[PACKET_FROM+1] >> 4);
  decodedPacket.fromPort = packet.payload[PACKET_FROM+1] &0xF;
  
  //To 12bit node id, 4 bit plug id
  decodedPacket.toNode = packet.payload[PACKET_TO];
  decodedPacket.toNode = decodedPacket.toNode << 4;
  decodedPacket.toNode = decodedPacket.toNode | (packet.payload[PACKET_TO+1] >> 4);
  decodedPacket.toPort = packet.payload[PACKET_TO+1] &0xF;

 //packet id
  decodedPacket.id = packet.payload[PACKET_ID];

 //Command
  decodedPacket.command = packet.payload[PACKET_COMMAND];
  decodedPacket.checksum = packet.checksum;
  decodedPacket.ptr = packet.payload[PACKET_LENGTH] - 10;
   
  for(i=PACKET_PAYLOAD; i < packet.payload[PACKET_LENGTH]; i++){

    if(decodedPacket.type < 3) { //single packet TCP or UDP
        
          decodedPacket.payload[i-PACKET_PAYLOAD] = packet.payload[i];   
     
    } else if(decodedPacket.type == 3) {  //broadcast
        //todo
    }
  }
  return decodedPacket;
}

void debug_incoming_packet(DataPacket packet) 
{
  background(0);
  //println("show packet info");
  text("Packet Info",5,15);
  int x = 20;
  DataPacketDecoded packetDecoded = decode_packet(packet);
  text("  status: " + packetDecoded.status,10,x+=20);
  text("  length: " + packetDecoded.length,10,x+=20);
  text("settings: " + packetDecoded.settings,10,x+=20);
  text("    type: " + packetDecoded.type,10,x+=20); 
  text("fromNode: " + packetDecoded.fromNode,10,x+=20);
  text("fromPort: " + packetDecoded.fromPort,10,x+=20);
  text("  toNode: " + packetDecoded.toNode,10,x+=20);
  text("  toPort: " + packetDecoded.toPort,10,x+=20);
  text("     ttl: " + packetDecoded.ttl,10,x+=20);
  text("      id: " + packetDecoded.id,10,x+=20);
  text(" command: " + packetDecoded.command,10,x+=20);
  
  String message = "";
  String message2 = "";
  for(i=0; i < packetDecoded.ptr; i++){
    message += int(packetDecoded.payload[i]);
    message += ",";
    message2 += char(packetDecoded.payload[i]);
  } 
  text(" payload: " + message,10,x+=20);
  text(" payload: " + message2,10,x+=20);
  
  text("checksum: " + packetDecoded.checksum,10,x+=20);
  text("     ptr: " + packetDecoded.ptr,10,x+=20);//*/
}

Arduino

Protocol.h

//Device Codes

#define DVC_LM33           0x01
#define DVC_ST11           0x02
#define DVC_ST15           0x03
#define DVC_DOORCONTACT    0x04
#define DVC_FLIPSWITCH     0x05
#define DVC_LIGHT          0x06
#define DVC_MOTION         0x07
#define DVC_2LINECHAR      0x08
#define DVC_POWER30        0x08
#define DVC_POWER100       0x09
#define DVC_SMARTOUTLET    0x10
#define DVC_RGBLIGHT       0x11

//Command Codes

#define CMD_GET_VERSION    0x01
#define CMD_PING           0x3E
#define CMD_PONG           0xE3

#define CMD_ACK            0x11

#define CMD_READ           0xAA
#define CMD_WRITE          0xBB

#define CMD_PORTTYPE       0xCC

#define CMD_AUTOSENDSTART  0xC1
#define CMD_AUTOSENDSTOP   0xC2

#define CMD_VALUE          0x02

#define CMD_ERROR          0xFF

//@todo commands for different data types?

//decoded packet struct

typedef struct {
  uint8_t command;
 //if 255 = ready to send // 0 = resend last
  uint8_t length;
  uint8_t status;
  uint8_t settings; //01010101 //version //type direct or broadcast //ack //ttl //priority //multi
  uint8_t type; //1 single tcp, 2 single udp, 3 multipacket tcp
  uint16_t fromNode;
  uint8_t fromPlug;
  uint16_t toNode; //00 is base //xffff is broadcast
  uint8_t toPlug;
  uint8_t id;
  uint8_t ttl;
  
  uint8_t payload[48];
  uint16_t checksum;
  uint8_t ptr;
} DataPacketDecoded;


//simple data packet

typedef struct {
  uint8_t ptr;
  uint8_t port;
  uint8_t status;
  uint8_t payload[56];
  uint16_t checksum;
} DataPacket;

//payload type

typedef struct {
  uint8_t length;
  uint8_t data[56];
} Payload;

typedef struct {
  long start;
  uint8_t waiting;
  uint8_t ptr;
} ACKStatus;

typedef struct {
  uint8_t type;
  uint8_t status;
} Port;



//shortcuts to important bytes
//keeps from having to decode the whole thing to check simple parts
#define PACKET_LENGTH   0
#define PACKET_SETTINGS 1
#define PACKET_FROM     2
#define PACKET_TO       4
#define PACKET_TTL      4
#define PACKET_ID       6
#define PACKET_COMMAND  7
#define PACKET_PAYLOAD  8

//an enum might be better here
#define STATUS_CLEAR    0
#define STATUS_RECEVING 1
#define STATUS_RECEVIED 2
#define STATUS_WAITING  3
#define STATUS_READY    4
#define STATUS_SENDING  5
#define STATUS_SENT     6
#define STATUS_ACK      7
#define STATUS_SUCCESS  8

#define OFFSET_PACKET 10
#define OFFSET_HEADER 8
#define OFFSET_FOOTER 2

Basenode.pde

#include h> //checksum functions
#include "protocol.h"

#define NODE_ID 0x02

#define PLUG_A DVC_LM33
#define PLUG_B DVC_LM33
#define PLUG_C DVC_LM33
#define PLUG_D DVC_LM33

#define PACKET_BUFFER 10 //can be adujsted based on availbe space

//#define DEBUG 1;
#define PACKET_TIMEOUT 100

#define PORTTYPE_SERIAL 0
#define PORTTYPE_RF12   1

#define PORT_COUNT 2
#define PORT_0 PORTTYPE_SERIAL
#define PORT_1 PORTTYPE_RF12

//types
//ACK
//Single TCp //requires 2byte packet ID
//Single UDP
//Multi TCP //requires 2byte total ID, 2 byte Packet ID + byte current packet + byte total packets
//multi packet
//packet id
//current packets
//total packets
//

Port ports[] = {{PORTTYPE_SERIAL,STATUS_CLEAR},
                {PORTTYPE_RF12,STATUS_CLEAR}};



//list attached ports
//1 2 3 4 //rf12

DataPacket data[PACKET_BUFFER];

uint16_t checksum;
byte uniqueId = 0;
byte i;
DataPacket incomingPacket; //current incoming packet
//incomingPacket.ptr = 0;

DataPacket *sendingPacket;

boolean sending = false;
boolean receiving = false;
long packetTimer;;

DataPacket *getNewPacket() {
  for(i=0; i < PACKET_BUFFER; i++) {
    if(data[i].status == STATUS_CLEAR){
      
      return &data[i];
    } 
  }
  //Serial.println("all full");
  return 0;
}

uint8_t getId() 
{
  return uniqueId++; //loops when it overflows
}

uint16_t compressAddress(uint16_t node, uint8_t port) 
{
  return (node << 4) | (port & 0x0F);
}
//byte m = 1;
void send_packet_serial(DataPacket *packet)
{
  uint16_t dataChecksum = 0;
  //cause a problem
 
 // m++;
  
  for(i=0; i < (packet->payload[PACKET_LENGTH]-OFFSET_FOOTER); i++){

    dataChecksum = _crc16_update(dataChecksum, packet->payload[i]);
     //if((m == 3) && (i == 0)){
    //  Serial.print(packet->payload[0]-5,BYTE);
    // } else {
      Serial.print(packet->payload[i],BYTE); 
    // }
    //Serial.print(",");
  } 
  
  packet->checksum = dataChecksum;
  Serial.print(highByte(dataChecksum),BYTE);
 // Serial.print(",");
  Serial.print(lowByte(dataChecksum),BYTE);
  packet->status = STATUS_SENT;
 //Serial.println("");
}

void send_packet_rf12(DataPacket *packet)
{
  /** //check address only 0-30 allowed
  
  *  @todo
  */
}

void process_command(DataPacket *packet)
{
  switch (packet->payload[PACKET_COMMAND]) {
    case CMD_PING:
      
      DataPacket *reply = build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, get_payload(packet));
      reply->status = STATUS_READY;
      
      //change status to ready to send
      
      break;
    //default:
      //send_packet(COMMAND_ERROR,NULL,0);
   //   break;
    }
}

//build rf12 packet


void build_incoming_packet(uint8_t byteIn)
{
   //mark which port it is coming from
 
  if(incomingPacket.ptr == 0){  //Packet length
  
    if(byteIn == 0) {
      //resend last
      sendingPacket->status = STATUS_SENDING;
    
    
    return;
    }
    if(byteIn == 255) {
      sendingPacket->status = STATUS_SUCCESS;
      sending = false; //allow next packet to send
      //aknowledge

      return;
    }
    if (byteIn > 128) {
      //Serial.flush();
      Serial.print(0,BYTE);
      
      return;
    }
    receiving = true;
    packetTimer = millis();
    incomingPacket.payload[PACKET_LENGTH] = byteIn;
    incomingPacket.status = STATUS_RECEVING;
    checksum = 0;
  }
  
  //checksum 
  if(incomingPacket.ptr < (incomingPacket.payload[PACKET_LENGTH] - OFFSET_FOOTER)){ //add to check sum
    incomingPacket.payload[incomingPacket.ptr] = byteIn;
    checksum = _crc16_update(checksum, byteIn);
    incomingPacket.ptr++;
    return;
  } else if(incomingPacket.ptr == (incomingPacket.payload[PACKET_LENGTH] - OFFSET_FOOTER)){ //get first byte of checksum
    incomingPacket.checksum = byteIn;
    incomingPacket.ptr++;
    return;
  } else { //get second byte and check the checksum
    incomingPacket.checksum = (incomingPacket.checksum << 8) | (byteIn & 0xFF);         
    if(checksum != incomingPacket.checksum) {
      //Serial.println(checksum, DEC);  
      //Serial.println(incomingPacket.checksum, DEC);   
      
       DataPacket *p = build_udp_packet(NODE_ID, 1, 0, 0, CMD_ERROR, packet_to_payload(&incomingPacket));
       p->status = STATUS_READY;
      Serial.flush(); 
      Serial.print(1,BYTE); //tell the node to resend last
      
      
    } else {
      incomingPacket.status=STATUS_RECEVIED;
      DataPacket *packet = getNewPacket();
      *packet = incomingPacket;
      //Serial.flush();
      Serial.print(255,BYTE); //tell the node the packet was successful
      
    }
    //reset incomingPacket
    receiving = false;
    incomingPacket.status=STATUS_CLEAR;
    incomingPacket.ptr = 0;
  }
  
  

}


//determine where to send packet. how to expand when we have 4 serial ports?

void process_stack(){
  
  //sending = false; //only send one pack per cycle
  
  for(i=0;i//check for packets to send


//check for packets to resend if ACKs are late
  
  
//check for data packets that can be deleted from the stack 
}



void process_packet(DataPacket *packet){
  switch (packet->status){
    case STATUS_CLEAR:
      #ifdef DEBUG
      Serial.println("CLEAR");
      #endif
      break;
    case STATUS_RECEVING:
      #ifdef DEBUG
      Serial.println("RECEVING");
      #endif
      break;
    case STATUS_RECEVIED:
      process_command(packet);
      packet->status = STATUS_CLEAR;
      #ifdef DEBUG
      Serial.println("RECEVIED");
      #endif
      break;
    case STATUS_WAITING:
      #ifdef DEBUG
      Serial.println("WAITING");
      #endif
      break;
    case STATUS_READY:
      //check for free port
      if(sending == false){
        packet->status = STATUS_SENDING;
        sendingPacket = packet;
        sending = true;
      }
      //if reset pointer and set to send
      #ifdef DEBUG
      Serial.println("READY");
      #endif
      break;
    case STATUS_SENDING:
      //because of the above, only one packet will be changed to sending
      //if(get_node(packet,PACKET_FROM) < 30){
        // send_packet_rf12(packet); 
      //} else {
         send_packet_serial(packet); 
     // }
      packet->status = STATUS_SENT;
      #ifdef DEBUG
      
      Serial.println("SENDING");
      #endif
      break;
    case STATUS_SENT:
      //if is a ack packet 
      if(packet->payload[PACKET_SETTINGS] == 1){
        packet->status = STATUS_ACK;
        //start packets ack timer
      } else {
        //packet->status = STATUS_SUCCESS;
      }
      #ifdef DEBUG
      sending = false; //allow next packet to send
      packet->status = STATUS_SUCCESS;
      Serial.println("SENT");
      #endif
      break;
    case STATUS_ACK:
       //if execeded timer resen
       //count resends if exceds max kill packet and add error
       #ifdef DEBUG
       Serial.println("ACK");
       #endif
      break;
    case STATUS_SUCCESS:
      //option to post status to screen
      packet->status = STATUS_CLEAR;
      #ifdef DEBUG
      Serial.println("SUCCESS");
      #endif
      break; 
  }
}

unsigned counter = 0;

void setup()
{
  Serial.begin(115200);
  //Serial.println("Ping:");
 //Serial.print("nodeID: ");
 // Serial.println(NODE_ID,DEC);
 // int sendPacketId =  build_udp_packet(NODE_ID, 1, 0, 0, CMD_PING, (byte*)"TEST TEST TEST", 1);
 // int sendPacketId2 = build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, (byte*)"HA HA HA", 8);
  
  //send_packet_serial(sendPacketId);
  //Serial.println("");
  /**/
  //DataPacket *p = build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, str_to_payload("TEEEST"));
 // send_packet_serial(p);
 // debug_packet(p);
  
}

void loop() {
 
  //byte message[1] = {uniqueId};
  //message[15] = uniqueId;
  //int sendPacketId =  build_udp_packet(NODE_ID, 1, 0, 0, CMD_PING, message, 1);
  // int sendPacketId2 = build_udp_packet(NODE_ID, 1, 0, 0, CMD_PONG, (byte*)"HA HA HA", 8);

  if(counter > 50000) {
    DataPacket *p = build_udp_packet(NODE_ID, 1, 0, 0, CMD_VALUE, str_to_payload("ha ha ha22222"));
    p->status = STATUS_READY;
    counter = 0;
  }
  counter++;

  process_stack();
  
  //recieve timer
  
  
  //and send command to resend last
  int byteIn;
  if (Serial.available()>0) {
    byteIn =  Serial.read();
    build_incoming_packet(byteIn);
  }
  process_stack();
  if(receiving == true) {
    if((millis() - packetTimer) > PACKET_TIMEOUT) {
      //Serial.flush();
      Serial.print(2,BYTE); //tell the node to resend last
      //DataPacket *p = build_udp_packet(NODE_ID, 1, 0, 0, CMD_ERROR, packet_to_payload(&incomingPacket));
      // p->status = STATUS_READY;
      receiving = false;
    }
  }

  //delay(5);     
}

/*FINISHED*/
Payload *get_payload(DataPacket *packet) 
{
  Payload payload;
  payload.length = packet->payload[PACKET_LENGTH] - OFFSET_PACKET;
  for(i=0; i < payload.length; i++){
    if(packet->payload[PACKET_SETTINGS] < 3) { //single packet TCP or UDP
      payload.data[i] = packet->payload[i+OFFSET_HEADER];   
     
    } else if(packet->payload[PACKET_SETTINGS] == 3) {  //broadcast
        //todo
    }
  }
  return &payload; 
}

Payload *packet_to_payload(DataPacket *packet) 
{
  Payload payload;
  payload.length = packet->payload[PACKET_LENGTH];
  for(i=0; i < (payload.length - OFFSET_FOOTER); i++){
    payload.data[i] = packet->payload[i];   
  }
  payload.data[payload.length - OFFSET_FOOTER] = highByte(packet->checksum);
  payload.data[payload.length - OFFSET_FOOTER + 1] = lowByte(packet->checksum);
  return &payload; 
}
 
uint16_t get_node(DataPacket *packet, uint8_t location)
{
  uint16_t node;  
  node = packet->payload[location];
  node = node << 4;
  node = node | (packet->payload[location+1] >> 4);
  return node;
}
 
 
Payload *str_to_payload(char string[])
{
  Payload payload;
  i=0;
  while(string[i] != 0){
    payload.data[i] = byte(string[i]); 
    i++;  
  }
  payload.length = i;
  //Serial.print("sizeOf: ");
 // Serial.println(i,DEC);
  return &payload; 
}

Payload *long_to_payload(long number)
{
  Payload payload;
  i=(sizeof(number)-1);
  payload.length = i;
  while(i != 0){
    payload.data[i] = number & 0xFF; 
    number = number >> 8;
    i--;  
  }
  return &payload; 
}

Payload *uint16_to_payload(uint16_t number)
{
  Payload payload;
  
  payload.length = 2;
   payload.data[0] = highByte(number);
   payload.data[1] = lowByte(number);
  
  return &payload; 
}

DataPacket *build_tcp_packet(uint16_t fromNode, uint8_t fromPlug, uint16_t toNode, uint8_t toPlug, uint8_t command, Payload *payload)
{
  DataPacket *packet = getNewPacket();
  //*packet.status = STATUS_READY;
  packet->status = STATUS_WAITING;
  packet->payload[PACKET_LENGTH] = payload->length + OFFSET_PACKET; //header 8 crc 2
  packet->payload[PACKET_SETTINGS] = 1;
  uint16_t address = compressAddress(fromNode, fromPlug);
  packet->payload[PACKET_FROM] = (address >> 8) & 0xFF;
  packet->payload[PACKET_FROM+1] = address & 0xFF;
  address = compressAddress(toNode, toPlug);
  packet->payload[PACKET_TO ] = (address >> 8) & 0xFF;
  packet->payload[PACKET_TO+1] = address & 0xFF;
  packet->payload[PACKET_ID] = getId();
  packet->payload[PACKET_COMMAND] = command;
  
  for(i=0;i < payload->length;i++){
    packet->payload[PACKET_PAYLOAD+i] = payload->data[i];
  }

  return packet;
}

/*
 * @return DataPacket * Pointer to the New DataPacket
 */

DataPacket *build_udp_packet(uint16_t fromNode, uint8_t fromPlug, uint16_t toNode, uint8_t toPlug, uint8_t command, Payload *payload)
{
  DataPacket *packet = getNewPacket();
  //*packet.status = STATUS_READY;
  packet->status = STATUS_WAITING;
  packet->payload[PACKET_LENGTH] = payload->length + OFFSET_PACKET; //header 8 crc 2
  packet->payload[PACKET_SETTINGS] = 2;
  uint16_t address = compressAddress(fromNode, fromPlug);
  packet->payload[PACKET_FROM] = (address >> 8) & 0xFF;
  packet->payload[PACKET_FROM+1] = address & 0xFF;
  address = compressAddress(toNode, toPlug);
  packet->payload[PACKET_TO ] = (address >> 8) & 0xFF;
  packet->payload[PACKET_TO+1] = address & 0xFF;
  packet->payload[PACKET_ID] = getId();
  packet->payload[PACKET_COMMAND] = command;
  
  for(i=0;i < payload->length;i++){
  packet->payload[PACKET_PAYLOAD+i] = payload->data[i];
  }
  
  return packet;
}

DataPacket *build_broadcast_packet(uint16_t fromNode, uint8_t fromPlug, uint8_t ttl, Payload payload)
{
  DataPacket *packet = getNewPacket();
  packet->status = STATUS_WAITING;
  return packet;
}

/**
 * turns a packet into a decoded packet
 */
DataPacketDecoded decode_packet(DataPacket *packet)
{
  DataPacketDecoded decodedPacket;
     
  decodedPacket.ttl = 0;
  
  //Set Packet length
  decodedPacket.length = packet->payload[PACKET_LENGTH];
  
  decodedPacket.status = packet->status;
  //settings 
  decodedPacket.settings = packet->payload[PACKET_SETTINGS];
  decodedPacket.type = packet->payload[PACKET_SETTINGS]; //right now these are the same things
  //From 12bit node id, 4 bit plug id
  decodedPacket.fromNode = packet->payload[PACKET_FROM];
  decodedPacket.fromNode = decodedPacket.fromNode << 4;
  decodedPacket.fromNode = decodedPacket.fromNode | (packet->payload[PACKET_FROM+1] >> 4);
  decodedPacket.fromPlug = packet->payload[PACKET_FROM+1] &0xF;
  //To 12bit node id, 4 bit plug id
  decodedPacket.toNode = packet->payload[PACKET_TO];
  decodedPacket.toNode = decodedPacket.toNode << 4;
  decodedPacket.toNode = decodedPacket.toNode | (packet->payload[PACKET_TO+1] >> 4);
  decodedPacket.toPlug = packet->payload[PACKET_TO+1] &0xF;
 //packet id
  decodedPacket.id = packet->payload[PACKET_ID];
 //Command
  decodedPacket.command = packet->payload[PACKET_COMMAND];
  
  decodedPacket.ptr = packet->payload[PACKET_LENGTH] - OFFSET_PACKET;
  
  
  Payload *payload = get_payload(packet);
   
  for(i=0; i < payload->length; i++){
    if(decodedPacket.type < 3) { //single packet TCP or UDP 
          decodedPacket.payload[i] = payload->data[i];   
    } else if(decodedPacket.type == 3) {  //broadcast
        //todo
    }
  }
  decodedPacket.checksum=packet->checksum;
  return decodedPacket;
}

void debug_packet(DataPacket *packet){
 DataPacketDecoded sendPacketedDecoded = decode_packet(packet);
  
  Serial.print("    sent: ");
  Serial.println(sendPacketedDecoded.status,DEC);
  Serial.print("  length: ");
  Serial.println(sendPacketedDecoded.length,DEC);
  Serial.print("settings: ");
  Serial.println(sendPacketedDecoded.settings,DEC);
  Serial.print("    type: ");
  Serial.println(sendPacketedDecoded.type,DEC); 
  Serial.print("fromNode: ");
  Serial.println(sendPacketedDecoded.fromNode,DEC);
  Serial.print("fromPlug: ");
  Serial.println(sendPacketedDecoded.fromPlug,DEC);
  Serial.print("  toNode: ");
  Serial.println(sendPacketedDecoded.toNode,DEC);
  Serial.print("  toPlug: ");
  Serial.println(sendPacketedDecoded.toPlug,DEC);
  Serial.print("     ttl: ");
  Serial.println(sendPacketedDecoded.ttl,DEC);
  Serial.print("      id: ");
  Serial.println(sendPacketedDecoded.id,DEC);
  Serial.print(" command: ");
  Serial.println(sendPacketedDecoded.command,DEC);
  Serial.print(" payload: ");
  for(i=0; i < sendPacketedDecoded.ptr; i++){
    Serial.print(sendPacketedDecoded.payload[i],DEC);
    Serial.print(",");
  } 
   Serial.print(" payload: ");
  for(i=0; i < sendPacketedDecoded.ptr; i++){
    Serial.print(sendPacketedDecoded.payload[i],BYTE);
    Serial.print(",");
  } 
  Serial.println("");
  Serial.print("checksum: ");
  Serial.println(sendPacketedDecoded.checksum,DEC);
  Serial.print("     ptr: ");
  Serial.println(sendPacketedDecoded.ptr,DEC);
}