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

#ifdef PWM_OUTPUT_TEST
PwmGenerator pwmOutputTest;
uint32_t pwmOutputTestPulseNs = PWM_OUTPUT_TEST_MIN_PULSE_NS;
uint32_t pwmOutputTestUpdatedMs = 0;

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

  pwmOutputTest.begin();
  ActualPwm actual = {};
  if (pwmOutputTest.start(PWM_OUTPUT_TEST_FREQUENCY_HZ,
                          pwmOutputTestPulseNs, actual)) {
    Serial.printf("PWM OUTPUT TEST STARTED: actual=%luHz pulse=%luns bits=%u\n",
      actual.actualHz, actual.actualPulseNs, 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_PULSE_NS + PWM_OUTPUT_TEST_MAX_PULSE_NS) * 0.5f;
  const float amplitude = (PWM_OUTPUT_TEST_MAX_PULSE_NS - PWM_OUTPUT_TEST_MIN_PULSE_NS) * 0.5f;
  const uint32_t pulseNs = static_cast<uint32_t>(center + amplitude * sinf(phase) + 0.5f);
  if (pulseNs == pwmOutputTestPulseNs) return;

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

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