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

// Peripherals
LCDWrapper screen = LCDWrapper(0x20, 16, 2);
XBeeWrapper xbee = XBeeWrapper();

// ACTUAL DATA
uint8_t wave_id     = 255; // last wave
uint8_t dist        = 255; // actual distance
Packet recv, send;

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

	screen.begin();
	screen.printf("[  node ready  ]");

	xbee.begin(38400);

}

void loop() {

	// 1. ignore if no received data
	if( xbee.receive(recv) != XBWRECV_OK )
		return;


	uint8_t opcode = recv.getOpcode();

	// DISCOVER REQUEST
	if( opcode == 0 ) {

		// security
		if( recv.getDist() == 0 && recv.getSender() != 255 )
			return;

		screen.printfn(0, "dsc[%3d] %3d / %3d", recv.getSender(), recv.getDist(), recv.getWave());

		// ignore if same wave
		if( recv.getWave() == wave_id ){
			screen.printfn(1, "x");
			return;
		}

		// update features
		wave_id = recv.getWave();
		dist    = recv.getDist()+1;

		// propagate wave
		send.setOpcode(0);
		send.setWave(wave_id);
		send.setDist(dist);
		send.setSender(42);

		screen.printfn(1, "dsc[%3d] %3d / %3d", send.getSender(), send.getDist(), send.getWave());
		xbee.broadcast(send);

	// MESSAGE DATA
	} else {

		screen.clear();
		screen.printfn(0, "msg[%3d/%3d] %3d", recv.getDist(), recv.getTTL(), recv.getSize());
		delay(500);
		screen.printfn_overflow(0, "%s", recv.getData());

		// ignore
		if( recv.getTTL() <= 0 || recv.getDist() > dist ){
			screen.printfn(1, "x");
			return;
		}

		// propagation
		send.setOpcode(1);
		send.setTTL(recv.getTTL()-1);
		send.setDist(dist);
		send.setData(recv.getData(), recv.getSize());

		screen.printfn(1, "msg[%3d/%3d] %3d", send.getDist(), send.getTTL(), send.getSize());
		xbee.broadcast(send);

	}

}