#include "hardware/pwm.h"
#include "pico/stdlib.h"

#include "sidetone.h"

Sidetone::Sidetone(uint gpio) : m_gpio(gpio)
{
    gpio_set_function(gpio, GPIO_FUNC_PWM);
    uint slice_num = pwm_gpio_to_slice_num(gpio);
    pwm_config config = pwm_get_default_config();
    pwm_init(slice_num, &config, true);
}

void Sidetone::on(uint freq)
{
    // Calculate and cofigure new clock divider according to the frequency
    // This formula is assuming we are running at 125MHz.
    // TODO: Make this work for any frequency
    float clkdiv = (1.f / freq) * 2000.f;
    uint slice_num = pwm_gpio_to_slice_num(m_gpio);
    pwm_set_clkdiv(slice_num, clkdiv);
    // Configure duty to 50% ((2**16)-1)/2) to generate a square wave
    pwm_set_gpio_level(m_gpio, 32768U);
}

void Sidetone::off() { pwm_set_gpio_level(m_gpio, 0); }