#include "App.h"
#include "Config.h"
#include <math.h>

#ifdef PWM_OUTPUT_TEST
PwmGenerator pwmOutputTest;
uint8_t pwmOutputTestDuty = 50;
uint32_t pwmOutputTestUpdatedMs = 0;

void setup() {
  Serial.begin(SERIAL_BAUD);
  delay(200);
  Serial.printf("\nPWM OUTPUT TEST: GPIO=%u requested=%luHz duty=%u..%u%% sine=%lums safe=%s\n",
    GPIO_PWM, PWM_OUTPUT_TEST_FREQUENCY_HZ,
    PWM_OUTPUT_TEST_MIN_DUTY_PCT, PWM_OUTPUT_TEST_MAX_DUTY_PCT,
    PWM_OUTPUT_TEST_SWEEP_PERIOD_MS,
    PWM_SAFE_LEVEL == HIGH ? "HIGH" : "LOW");

  pwmOutputTest.begin();
  ActualPwm actual = {};
  if (pwmOutputTest.start(PWM_OUTPUT_TEST_FREQUENCY_HZ,
                          pwmOutputTestDuty, actual)) {
    Serial.printf("PWM OUTPUT TEST STARTED: actual=%luHz duty=%.2f%% bits=%u\n",
      actual.actualHz, actual.actualDutyPct, actual.bits);
  } else {
    Serial.println("PWM OUTPUT TEST FAILED");
  }
}

void loop() {
  const uint32_t now = millis();
  if (now - pwmOutputTestUpdatedMs < PWM_OUTPUT_TEST_UPDATE_MS) return;
  pwmOutputTestUpdatedMs = now;

  constexpr float PWM_TWO_PI = 6.28318530718f;
  const float phase = PWM_TWO_PI * (now % PWM_OUTPUT_TEST_SWEEP_PERIOD_MS) /
    PWM_OUTPUT_TEST_SWEEP_PERIOD_MS;
  const float center = (PWM_OUTPUT_TEST_MIN_DUTY_PCT + PWM_OUTPUT_TEST_MAX_DUTY_PCT) * 0.5f;
  const float amplitude = (PWM_OUTPUT_TEST_MAX_DUTY_PCT - PWM_OUTPUT_TEST_MIN_DUTY_PCT) * 0.5f;
  const uint8_t duty = static_cast<uint8_t>(center + amplitude * sinf(phase) + 0.5f);
  if (duty == pwmOutputTestDuty) return;

  ActualPwm actual = {};
  if (pwmOutputTest.start(PWM_OUTPUT_TEST_FREQUENCY_HZ, duty, actual)) {
    pwmOutputTestDuty = duty;
  } else {
    Serial.printf("PWM OUTPUT TEST UPDATE FAILED: duty=%u%%\n", duty);
    delay(100);
  }
}
#else
App app;

void setup() { app.begin(); }
void loop() { app.update(); }
#endif