#include <Wire.h>
#include "lcd.h"
#include "xbee_wrapper.h"
#include "packet.h"

#define WAVE_TIMEOUT 5000

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

// ACTUAL DATA
uint8_t wave_id = 250;
Packet recv, send;
unsigned long time;

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

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

	xbee.begin(38400);

	time = millis();
}

void loop() {

	/* [1] if WAVE_TIMEOUT reached */
	if( millis() - time >= WAVE_TIMEOUT ){
		time = millis();

		// increment wave id (will overflow from 255 to 0)
		send.setOpcode(0);
		send.setWave(++wave_id);
		send.setDist(0);
		send.setSender(42);

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

		if( xbee.broadcast(send) == XBWSEND_OK ) screen.printfn(1, "sent");
		else                                     screen.printfn(1, "failed");
	}

	/* [2] Listen for incoming data */
	if( xbee.receive(recv) != XBWRECV_OK )
		return;

	uint8_t opcode = recv.getOpcode();

	// ignore DISCOVER requests
	if( opcode == 0 )
		return;

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

}