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

//Setup
Node myself = Node();

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

// ACTUAL DATA
uint8_t wave_id = 250;
unsigned long time;

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

	screen.begin();
	screen.printfn(0, "[  well ready  ]");

	xbee.begin(38400);

	time = millis();
}

void loop() {

	/* [1] if DISCOVER_TTL reached -> send discover request */
	if( millis() - time >= DISCOVER_TTL ){
		time = millis();

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

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

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

	/* [2] Listen for incoming data */
  Packet recv;
	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", (char*) recv.getData());

}