#include "Dezibot.h"

std::Dezibot dezibot;

void setup() {
  dezibot.begin();
  Serial.begin(115200);
  // Wait for Serial to init
  while (!Serial) {
    ;
    ;
  }
  Serial.println("Starting Power Management Test");
  Serial.println("Status on end of setup:");
  
  dezibot.power.dumpPowerStatistics();
  dezibot.power.dumpConsumerStatistics();
}

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

void loop() {
  dezibot.multiColorLight.setLed(ALL, 0x00FFFFFF);
  Serial.println("Turned on all RGB LEDs");
  dumpInfoAndWait();
  
  dezibot.motion.move();
  Serial.println("Turned on all motors");
  dumpInfoAndWait();
  
  dezibot.infraredLight.bottom.turnOn();
  Serial.println("Turned on bottom IR LEDs");
  dumpInfoAndWait();
  
  dezibot.multiColorLight.turnOffLed(ALL);
  dezibot.motion.stop();
  dezibot.infraredLight.bottom.turnOff();
  Serial.println("Turned off all LEDs and motors");
  dumpInfoAndWait();
}