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

// 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
Packet recv;
Packet send;

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(recv) == XBWRECV_OK ){
		uint8_t opcode = recv.getOpcode();
		Serial.print("+ opcode = ");
		Serial.println(opcode);

		// DISCOVER REQUEST
		if( opcode == 0 ) {
			Serial.println("+ discover request");
			Serial.print("  + wave = ");
			Serial.println(recv.getWave());
			Serial.print("  + dist = ");
			Serial.println(recv.getDist());

			if( recv.getWave() != wave_id ){
				wave_id = recv.getWave();
				dist    = recv.getDist()+1;
				recv.setDist(dist);

				send.setOpcode(0);
				send.setWave(wave_id);
				send.setDist(dist);
				xbee.broadcast(send);
				Serial.println("  + propagated");

			}

		// MESSAGE DATA
		} else {

			Serial.println("+ message data");
			Serial.print("  + ttl = ");
			Serial.println(recv.getTTL());
			Serial.print("  + dist = ");
			Serial.println(recv.getDist());

			// propagation
			if( recv.getTTL() > 0 && recv.getDist() <= dist ){
				send.setOpcode(1);
				send.setTTL(recv.getTTL()-1);
				send.setDist(dist);
				send.setData(recv.getData(), recv.getSize());
				xbee.broadcast(send);
				Serial.println("  + propagated");
			}

		}


		// manage packet
	}

}