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

// Peripherals
LiquidCrystal_I2C screen(0x20, 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();
	screen.home();
	screen.setCursor(0, 0);
	screen.print("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;


		status(wave_id, dist);
		screen.print("dsc");
		screen.print("[");
		screen.print(recv.getSender());
		screen.print("] -> ");
		screen.print(recv.getDist());
		screen.print("/");
		screen.print(recv.getWave());

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

			status(wave_id, dist);
			screen.print("propagated");
			delay(100);

			send.setOpcode(0);
			send.setWave(wave_id);
			send.setDist(dist);
			send.setSender(42);
			xbee.broadcast(send);
			Serial.print("  + send discover[");
			Serial.print(send.getSender());
			Serial.print("] -> ");
			Serial.print(send.getDist());
			Serial.print(" / ");
			Serial.println(send.getWave());

		}

	// MESSAGE DATA
	} else {

		screen.clear();
		screen.setCursor(0,0);
		screen.print("message(");
		screen.print(recv.getDist());
		screen.print(",");
		screen.print(recv.getTTL());
		screen.print(",");
		screen.print(recv.getSize());
		screen.print(")");
		screen.setCursor(0,1);
		screen.print( (char*)recv.getData());
		delay(600);


		// 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.print("  + send message(");
			Serial.print(recv.getDist());
			Serial.print(", ");
			Serial.print(recv.getTTL());
			Serial.print(", ");
			Serial.print(recv.getSize());
			Serial.println(")");
		}

	}

}



void status(uint8_t wave, uint8_t dist){
	screen.clear();
	screen.setCursor(0,0);
	screen.print(dist);
	screen.print(" / ");
	screen.print(wave);
	screen.setCursor(0,1);
}