#include "stm32c031xx.h"

/* ---- ADC bits defined manually in case header doesn't expose them ---- */
#define ADC_CR_ADEN_BIT        (1U << 0)
#define ADC_CR_ADDIS_BIT       (1U << 1)
#define ADC_CR_ADSTART_BIT     (1U << 2)
#define ADC_CR_ADSTP_BIT       (1U << 4)
#define ADC_CR_ADVREGEN_BIT    (1U << 28)
#define ADC_CR_ADCAL_BIT       (1U << 31)

#define ADC_ISR_ADRDY_BIT      (1U << 0)
#define ADC_ISR_EOC_BIT        (1U << 2)

#define SERVO_MIN  100U   /* 1.0 ms pulse */
#define SERVO_MAX  200U   /* 2.0 ms pulse */

static void small_delay(volatile uint32_t n)
{
    while (n--) {
        __asm__("nop");
    }
}

static void pwm_servo_init(void)
{
    /* Enable GPIOA clock */
    RCC->IOPENR |= RCC_IOPENR_GPIOAEN;

    /* PA6 -> Alternate Function */
    GPIOA->MODER &= ~(3U << (6U * 2U));
    GPIOA->MODER |=  (2U << (6U * 2U));

    /* PA6 -> AF1 = TIM3_CH1 */
    GPIOA->AFR[0] &= ~(0xFU << (6U * 4U)); // GPIOA Alternate Function Register LOW (AFRL)
    GPIOA->AFR[0] |=  (1U   << (6U * 4U));

    /* Enable TIM3 clock */
    RCC->APBENR1 |= RCC_APBENR1_TIM3EN;

    /*
      Timer clock:
      48 MHz / 480 = 100 kHz  -> 1 tick = 10 us
      ARR = 2000 -> 20 ms period -> 50 Hz
    */
    TIM3->PSC = 480U - 1U;
    TIM3->ARR = 2000U - 1U;

    /* PWM mode 1 on CH1 */
    TIM3->CCMR1 &= ~(7U << 4);
    TIM3->CCMR1 |=  (6U << 4);

    /* Preload */
    TIM3->CCMR1 |= TIM_CCMR1_OC1PE;
    TIM3->CR1   |= TIM_CR1_ARPE;

    /* Enable CH1 output */
    TIM3->CCER |= TIM_CCER_CC1E;

    /* Start centered */
    TIM3->CCR1 = 150U;

    /* Update registers */
    TIM3->EGR |= TIM_EGR_UG;

    /* Start timer */
    TIM3->CR1 |= TIM_CR1_CEN;
}

static void adc_init_pa0(void)
{
    /* Enable GPIOA clock */
    RCC->IOPENR |= RCC_IOPENR_GPIOAEN;

    /* PA0 -> analog mode */
    GPIOA->MODER |= (3U << (0U * 2U));

    /* Enable ADC clock */
    RCC->APBENR2 |= RCC_APBENR2_ADCEN;

    /* Enable regulator */
    ADC1->CR |=  ADC_CR_ADVREGEN_BIT;

    small_delay(20000);

    /* Ensure ADC disabled before calibration */
    if (ADC1->CR & ADC_CR_ADEN_BIT) {
        ADC1->CR |= ADC_CR_ADDIS_BIT;
        while (ADC1->CR & ADC_CR_ADEN_BIT) {
        }
    }

    /* Calibrate ADC */
    ADC1->CR |= ADC_CR_ADCAL_BIT;
    while (ADC1->CR & ADC_CR_ADCAL_BIT) {
    }

    /* Select channel 0 (PA0) */
    ADC1->CHSELR = (1U << 0);

    /* Enable ADC */
    ADC1->ISR |= ADC_ISR_ADRDY_BIT;
    ADC1->CR  |= ADC_CR_ADEN_BIT;

    while (!(ADC1->ISR & ADC_ISR_ADRDY_BIT)) {
    }
}

static uint16_t adc_read(void)
{
    /* Start conversion */
    ADC1->CR |= ADC_CR_ADSTART_BIT;

    /* Wait end of conversion */
    while (!(ADC1->ISR & ADC_ISR_EOC_BIT)) {
    }

    /* Read result */
    return (uint16_t)(ADC1->DR & 0xFFFFU);
}

int main(void)
{
    uint16_t adc_value;
    uint32_t ccr_value;

    pwm_servo_init();
    adc_init_pa0();

    while (1)
    {
        adc_value = adc_read();

        /*
          Map ADC 0..4095 to servo pulse 1.0..2.0 ms
          CCR1 100..200
        */
        ccr_value = SERVO_MIN +
                    ((uint32_t)adc_value * (SERVO_MAX - SERVO_MIN)) / 4095U;

        TIM3->CCR1 = ccr_value;
    }
}