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

#define SEND_TIMEOUT 10000

// 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
unsigned long time;
Packet recv, send;

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

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

	xbee.begin(38400);

	time = millis();
}

void loop() {

	/* [1] If SEND_TIMEOUT reached -> send data */
	if( millis() - time >= SEND_TIMEOUT ){
		time = millis();
		send_data();
	}


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

	uint8_t opcode = recv.getOpcode();

	// 1. manage discover request
	if( opcode == 0 )
		return manage_discover();

	// 2. manage message data
	else if( opcode == 1 )
		return manage_message();


}

void send_data(){
	// 1. prepare message
	send.setOpcode(1);
	send.setDist(dist);
	send.setTTL(MESSAGE_TTL);
	uint8_t data[19] = {0};
	memcpy(data, "https://git.xdrm.io", 19);
	send.setData(data, 19);
	send.setSender(SENDERID);

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

	// 3. send
	xbee.broadcast(send);
	screen.printfn(1, "sent            ");
}


void manage_discover(){
	// 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(SENDERID);

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

void manage_message(){
	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);
}