#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include "packet.h"       // Packet
#include "xbee_wrapper.h" // WBeeWrapper
#include "protocol.h"

// Peripherals
LiquidCrystal_I2C screen(0x27, 16, 2);
XBeeWrapper xbee = XBeeWrapper();

// ACTUAL DATA
uint8_t wave_id     = 255; // last wave
uint8_t dist        = 255; // actual distance
struct discover dsc = {0,0,0};
struct message msg  = {1, dist, 0, 0, {}};
Packet pkt;

void setup() {
  Serial.begin(38400);
  Serial.println("+ ready");

  screen.begin();
  screen.backlight();

  xbee.begin(38400);
}

void loop() {

	// 1. ignore if no received data
	if( xbee.receive(pkt) == XBWRCV_OK ){
		// manage packet
	}
	// xbee.readPacket();
	// if( xbee.getResponse().isAvailable() ){

	// 	if( xbee.getResponse().getApiId() == ZB_RX_RESPONSE ){
	// 		// got a zb rx packet

	// 		// now extract response
	// 		xbee.getResponse().getZBRxResponse(res);

	// 		// get data type (byte 0)
	// 		uint8_t type = res.getData(0)


	// 		res.getBytes()
	// 		analogWrite(dataLed, res.getData(0));
	// 	}

	// }

}