#include "Dezibot.h"

/*
 * Test case of varying power consumption, with logging of modeled state on
 * secondary UART.
 */

Dezibot dezibot;

// Using alternate Serial pins to not be powered by the USB port
#define RXD_HEADER 16
#define TXD_HEADER 17

// Task for independently toggling the bottom IR LED
void powerChange(void *pvParameters) {
    while (true) {
      dezibot.infraredLight.bottom.turnOn();
      delay(1000);
      dezibot.infraredLight.bottom.turnOff();
      delay(1000);
    }
  }
  
  // Tast for independently toggling the front IR LED
  void powerChange2(void *pvParameters) {
    while (true) {
      dezibot.infraredLight.front.turnOn();
      delay(1000);
      dezibot.infraredLight.front.turnOff();
      delay(1000);
    }
  }


void setup() {
  dezibot.begin();
  // Output CSV-Header for Timestamp and modelled current of all components
  // Start power consumption task
  xTaskCreate(powerChange, "powerChange", 4096, NULL, tskIDLE_PRIORITY, NULL);

  // Start second power consumption task
  xTaskCreate(powerChange2, "powerChange2", 4096, NULL, tskIDLE_PRIORITY, NULL);
}

void loop() {
  while (true) {
    dezibot.multiColorLight.turnOffLed();
    for (leds led : {TOP_LEFT, TOP_RIGHT, BOTTOM, TOP, ALL}) {
      dezibot.multiColorLight.setLed(led, 255, 0, 0);
      delay(1000);
      dezibot.multiColorLight.setLed(led, 0, 255, 0);
      delay(1000);
      dezibot.multiColorLight.setLed(led, 0, 0, 255);
      delay(1000);
    }
  }
}