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

#define SEND_TIMEOUT 10000

// ACTUAL DATA
unsigned long time;
Node myself = Node();

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

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

	screen.begin();
	screen.printfn(0, "[  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 */
  Packet recv;
	if( xbee.receive(recv) != XBWRECV_OK )
		return;

	uint8_t opcode = recv.getOpcode();

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

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


}

void send_data(){
	Serial.println(" -> message");
	// 1. prepare message
  Packet send;
	send.setOpcode(OPCODE_MESSAGE);
	send.setSender(SENDERID);
	send.setDist(myself.getDist());
	send.setTTL(MESSAGE_TTL);
	char data[64] = {0};
	sprintf(data, "%d has: dist %d / wave %d", SENDERID, myself.getDist(), myself.getWave());
	send.setData(data);

	// 2. broadcast
	xbee.sendToNearest(send);

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


void manage_discover(Packet recv){
	Serial.print(" <- discover[ ");
	Serial.print(recv.getDist());
	Serial.print(" / ");
	Serial.print(recv.getWave());
	Serial.println(" ]");

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

	// update features
	if( !myself.update(recv.getDist(), recv.getWave()) ){
		screen.printfn(1, "x               ");
		return;
	}

	// propagate wave
  Packet send;
	send.setOpcode(OPCODE_DISCOVER);
	send.setWave(myself.getWave());
	send.setDist(myself.getDist());

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

void manage_message(Packet recv){
	if( recv.getTTL() <= 0 )
		return;

	Serial.print(" <- message[");
	Serial.print(recv.getDist());
	Serial.print("/");
	Serial.print(recv.getTTL());
	Serial.print("] ");
	Serial.print(recv.getSize());
	Serial.print(" '");
	Serial.print((char*) recv.getData());
	Serial.println("'");

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

	// ignore
	if( recv.getDist() > myself.getDist() ){
		screen.printfn(1, "x               ");
		return;
	}

	// propagation
	recv.setTTL(recv.getTTL()-1);
	recv.setDist(myself.getDist());

	screen.printfn(1, "msg[%3d/%3d] %3d", recv.getDist(), recv.getTTL(), recv.getSize());
	delay(500);
	screen.printfn(1, "%s", (char*) recv.getData());
	xbee.sendToNearest(recv);
}