#include "stm32c031xx.h"

// Define servo control parameters
#define SERVO_PWM_PERIOD 20000        // Period in microseconds (20ms for 50Hz PWM)
#define MIN_PULSE_WIDTH 1000          // Minimum pulse width (1ms for 0°)
#define MAX_PULSE_WIDTH 2000          // Maximum pulse width (2ms for 180°)
#define TIMER_FREQUENCY 1000000      // Timer frequency (1 MHz)

void delay_ms(uint32_t ms) {
    // Simple delay function to create a time delay of 'ms' milliseconds
    while (ms--) {
        for (volatile uint32_t i = 0; i < 8000; i++) {  // Adjust the loop count based on your clock
        }
    }
}

void init_gpio(void) {
    // Enable GPIOA clock (the pin to be used for PWM)
    RCC->IOPENR |= (1<<0);

    // Set PA6 as an alternate function (AF1 for TIM3_CH1)
    //GPIOA->MODER &= ~GPIO_MODER_MODE0;
    GPIOA->MODER |= (1<<13);  // Set PA0 as alternate function
    GPIOA->AFR[0] &= ~(1<<12);  // Clear AF for PA6
    GPIOA->AFR[0] |= (1 << 0);  // Set AF1 for PA6 (TIM3_CH1)
}

void init_timer(void) {
    // Enable the clock for Timer 3 (TIM3)
    RCC->APBENR1 |= (1<<1);

    // Set the timer prescaler (assuming 1 MHz timer frequency)
    TIM3->PSC = 8 - 1;  // Set prescaler for 1 MHz timer

    // Set the auto-reload register (ARR) for 20ms period (50Hz)
    TIM3->ARR = SERVO_PWM_PERIOD - 1;

    // Set the pulse width for TIM3 (to control servo angle)
    TIM3->CCR1 = MIN_PULSE_WIDTH;  // Default to 1ms pulse (servo at 0°)

    // Set PWM mode and enable output
    TIM3->CCMR1 &= ~TIM_CCMR1_OC1M;
    TIM3->CCMR1 |= TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2;  // PWM mode 1
    TIM3->CCER |= TIM_CCER_CC1E;  // Enable TIM3 CH1 output

    // Enable the timer
    TIM3->CR1 |= TIM_CR1_CEN;
}

void set_servo_angle(uint16_t angle) {
    // Map the angle (0 to 180 degrees) to pulse width (1000 to 2000 microseconds)
    uint16_t pulse_width = MIN_PULSE_WIDTH + (angle * (MAX_PULSE_WIDTH - MIN_PULSE_WIDTH)) / 180;

    // Update the PWM pulse width (CCR1 register)
    TIM3->CCR1 = pulse_width;
}

int main(void) {
    // System initialization
    SystemInit();

    // Initialize GPIO for PWM output (PA0)
    init_gpio();

    // Initialize Timer for PWM generation
    init_timer();

    while (1) {
        // Sweep servo from 0 to 180 degrees and back
        for (uint16_t angle = 0; angle <= 180; angle++) {
            set_servo_angle(angle);  // Set servo position
            delay_ms(20);            // Delay to allow the servo to move
        }

        for (uint16_t angle = 180; angle >= 0; angle--) {
            set_servo_angle(angle);  // Set servo position
            delay_ms(20);            // Delay to allow the servo to move
        }
    }
}