#include <Wire.h>
#include <XBee.h>
#include "protocol.h"
#include <LiquidCrystal_I2C.h>

// CONSTANTS
#define WAVE_TIMEOUT 5000 // send wave every (in ms)

// Peripherals
LiquidCrystal_I2C screen(0x27, 16, 2);
XBee xbee = XBee();

// ACTUAL DATA
uint8_t wave_id = 250;
struct discover req = {0,0,0};

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

  screen.begin();
  screen.backlight();

  xbee.setSerial(Serial1);
  Serial1.begin(38400);

  // xbee.begin(38400);
}

void loop() {

  delay(WAVE_TIMEOUT);

  // increment wave id (will overflow from 255 to 0)
  req.wave = ++wave_id; // set wave id

  screen.clear();
  screen.print("+ wave");
  screen.print(req.wave);

  uint8_t payload[3] = {req.opcode, req.wave, req.dist};

  XBeeAddress64 addr64 = XBeeAddress64(0x00000000, 0x0000FFFF);
  Tx64Request tx = Tx64Request(addr64, payload, sizeof(payload));
  xbee.send(tx);

  /* Check the status of our sent payload (not mandatory)

    TxStatusResponse txStatus = TxStatusResponse();
    xbee.readPacket();
    if( xbee.getResponse().isAvailable() ){
      if( xbee.getResponse().getApiId() == TX_STATUS_RESPONSE ){
            xbee.getResponse().getTxStatusResponse(txStatus);
            txStatus.isSuccess() && Serial.println(" ... sent") || Serial.println(" ... failed");
      }
    }

  */
}