#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);
  }
}

// Task for periodic logging of power state
void outputCsvLine(void *pvParameters) {
  while (true) {
    Serial1.printf(
        "%d,%f,%f,%f,%f,%f,%d,%d,%d,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%"
        "f,%f,%f,%f\r\n",
        millis(), dezibot.power.getCurrentCurrent(),
        dezibot.power.getMax3V3Current(),
        dezibot.power.getBatteryChargePercent(),
        dezibot.power.getBatteryChargeCoulombs(),
        dezibot.power.getBatteryVoltage(), dezibot.power.isUSBPowered(),
        dezibot.power.isBatteryPowered(), dezibot.power.isBatteryCharging(),
        dezibot.power.isBatteryDischarging(),
        dezibot.power.isBatteryFullyCharged(),
        dezibot.power.getConsumerCurrent(PowerParameters::PowerConsumers::ESP),
        dezibot.power.getConsumerCurrent(PowerParameters::PowerConsumers::WIFI),
        dezibot.power.getConsumerCurrent(
            PowerParameters::PowerConsumers::LED_RGB_TOP_LEFT),
        dezibot.power.getConsumerCurrent(
            PowerParameters::PowerConsumers::LED_RGB_TOP_RIGHT),
        dezibot.power.getConsumerCurrent(
            PowerParameters::PowerConsumers::LED_RGB_BOTTOM),
        dezibot.power.getConsumerCurrent(
            PowerParameters::PowerConsumers::RGBW_SENSOR),
        dezibot.power.getConsumerCurrent(
            PowerParameters::PowerConsumers::LED_IR_FRONT),
        dezibot.power.getConsumerCurrent(
            PowerParameters::PowerConsumers::LED_IR_BOTTOM),
        dezibot.power.getConsumerCurrent(
            PowerParameters::PowerConsumers::PT_IR),
        dezibot.power.getConsumerCurrent(
            PowerParameters::PowerConsumers::PT_DL),
        dezibot.power.getConsumerCurrent(
            PowerParameters::PowerConsumers::LED_UV),
        dezibot.power.getConsumerCurrent(
            PowerParameters::PowerConsumers::DISPLAY_OLED),
        dezibot.power.getConsumerCurrent(
            PowerParameters::PowerConsumers::MOTOR_LEFT),
        dezibot.power.getConsumerCurrent(
            PowerParameters::PowerConsumers::MOTOR_RIGHT),
        dezibot.power.getConsumerCurrent(PowerParameters::PowerConsumers::IMU));
    delay(100);
  }
}

void setup() {
  dezibot.begin();
  Serial1.begin(115200, SERIAL_8N1, RXD_HEADER, TXD_HEADER);
  // Output CSV-Header for Timestamp and modelled current of all components
  Serial1.printf(
      "Timestamp (ms),Current (mA),calculated max current (mA),charge "
      "(%%),charge (C),voltage "
      "(V),isUSBPowered,isBatteryPowered,isBatteryCharging,"
      "isBatteryDischarging,isBatteryFullyCharged,ESP (mA),WIFI "
      "(mA),LED_RGB_TOP_LEFT (mA),"
      "LED_RGB_TOP_RIGHT (mA),LED_RGB_BOTTOM (mA),RGBW_SENSOR "
      "(mA),LED_IR_FRONT (mA),LED_IR_BOTTOM (mA),PT_IR (mA),PT_DL (mA),"
      "LED_UV (mA),DISPLAY_OLED (mA),MOTOR_LEFT (mA),MOTOR_RIGHT "
      "(mA),IMU (mA)\r\n");
  // Start logging task
  xTaskCreate(outputCsvLine, "outputCsvLine", 4096, NULL, tskIDLE_PRIORITY,
              NULL);

  // 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);
    }
  }
}