#include "Dezibot.h"
#define DEBUG

Dezibot dezibot = Dezibot();

void blip_io(int times) {
  constexpr int ioPin = 17;
  pinMode(ioPin, OUTPUT);
  for(int i = 0; i<times; i++) {
    digitalWrite(ioPin,1);
    delay(500);
    digitalWrite(ioPin,0);
    delay(500);
  }
}

void setup() {
  #ifdef DEBUG
  Serial.begin(112500);
  Serial.println("Debug enabled.");
  #endif
  dezibot.communication.begin();
  dezibot.communication.setGroupNumber(1);
  dezibot.communication.sendMessage("Repeated send power consumption test commencing");
  #ifdef DEBUG
  Serial.println("Mesh set up");
  /* Set up receive handler */
  dezibot.communication.onReceive(handle_receive);
  Serial.println("Set up receive. Printing incoming messages:");
  Serial.println("Sending broadcast messages to generate TX power consumption:");
  #endif
  blip_io(1);
  delay(5000);
  #ifdef DEBUG
  Serial.println("Starting Transmission...");
  #endif
  blip_io(2);
}

void handle_receive(String &message) {
  Serial.println(message);
}

void loop() {
    /* Continuously send to consume power on TX */
    dezibot.communication.sendMessage("Power Test Message");
    sleep(2);
}