#include "Dezibot.h"
#define HEADER_SERIAL
#define HEADER_SERIAL_RX_PIN 16
#define HEADER_SERIAL_TX_PIN 17

Dezibot dezibot;

void setup() {
  // Initialize all modules separately so header stays unused
  dezibot.multiColorLight.begin();
  dezibot.motion.begin();
  dezibot.infraredLight.begin();

  #ifdef HEADER_SERIAL
  Serial1.begin(115200, SERIAL_8N1, HEADER_SERIAL_RX_PIN, HEADER_SERIAL_TX_PIN);
  #else
  Serial1.begin(115200);
  #endif
  // Wait for Serial to init
  while (!Serial) {
    ;
    ;
  }
  Serial1.println("Starting Power Management Test");
  Serial1.println("Status on end of setup:");
  #ifdef Power_h
  dezibot.power.dumpPowerStatistics();
  dezibot.power.dumpConsumerStatistics();
  #endif
}

void dumpInfoAndWait() {
  delay(100);
  dezibot.power.dumpPowerStatistics();
  dezibot.power.dumpConsumerStatistics();
  delay(900);
}

void loop() {
  dezibot.multiColorLight.setLed(ALL, 0x00FFFFFF);
  Serial1.println("Turned on all RGB LEDs");
  #ifdef Power_h
  dumpInfoAndWait();
  #endif
  dezibot.motion.move();
  Serial1.println("Turned on all motors");
  #ifdef Power_h
  dumpInfoAndWait();
  #endif
  dezibot.infraredLight.bottom.turnOn();
  Serial1.println("Turned on bottom IR LEDs");
  #ifdef Power_h
  dumpInfoAndWait();
  #endif
}