void delay_ms(uint32_t time_ms);

void GPIO_Init(void) {
    // Enable clock for GPIOB
    RCC->AHBENR |= RCC_AHBENR_GPIOBEN;

    // Configure PB10 as alternate function (AF1 for TIM2_CH3)
    GPIOB->MODER &= ~(3U << (10 * 2));    // Clear mode bits for PB10
    GPIOB->MODER |= (2U << (10 * 2));     // Set PB10 to alternate function mode

    // Set alternate function to AF1 (TIM2_CH3)
    GPIOB->AFR[1] &= ~(0xF << ((10 - 8) * 4)); // Clear AF bits for PB10
    GPIOB->AFR[1] |= (1U << ((10 - 8) * 4));   // Set AF1 for PB10
}

void PWM_Init(void) {
    // Enable clock for TIM2
    RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;

    // Set prescaler: (72 MHz / 72) = 1 MHz timer clock
    TIM2->PSC = 71;  // Prescaler value: PSC + 1 = 72

    // Set auto-reload value: (1 MHz / 1000) = 1 kHz PWM frequency
    TIM2->ARR = 19999; // Auto-reload value

    // Set CC3 channel as output and set PWM mode 1
    TIM2->CCMR2 &= ~TIM_CCMR2_OC3M;                      // Clear output compare mode bits for channel 3
    TIM2->CCMR2 |= (0b110 << TIM_CCMR2_OC3M_Pos);        // Set PWM mode 1 on channel 3

    // Set Capture/Compare count (initial duty cycle)
    TIM2->CCR3 = 14999; // Start with LED fully on

    // Enable capture/compare output for channel 3
    TIM2->CCER |= TIM_CCER_CC3E;

    // Enable counter
    TIM2->CR1 |= TIM_CR1_CEN;
}

void moveServo(uint16_t startPulse, uint16_t endPulse, uint16_t stepSize, uint32_t stepDelay, uint8_t pauseAtEnd) {
    if (startPulse < endPulse) {
        for (uint16_t pulse = startPulse; pulse <= endPulse; pulse += stepSize) {
            TIM2->CCR3 = pulse;
            delay_ms(stepDelay);
        }
    } else {
        for (uint16_t pulse = startPulse; pulse >= endPulse; pulse -= stepSize) {
            TIM2->CCR3 = pulse;
            delay_ms(stepDelay);
        }
    }
    if (pauseAtEnd) {
        delay_ms(1000); // Pause for 1 second
    }
}

int main(void) {
    GPIO_Init(); // Initialize GPIO for PWM output
    PWM_Init();  // Initialize PWM

    uint16_t pulseMin = 999;   // -90 degrees
    uint16_t pulseMid = 1499;  // 0 degrees
    uint16_t pulseMax = 1999;  // +90 degrees

    while (1) {
        // Move from -90° to 0°
        moveServo(pulseMin, pulseMid, 1, 1, 1); // stepSize=1, stepDelay=1ms, pauseAtEnd=1

        // Move from 0° to +90°
        moveServo(pulseMid, pulseMax, 1, 1, 1);

        // Move from +90° to 0°
        moveServo(pulseMax, pulseMid, 1, 1, 1);

        // Move from 0° to -90°
        moveServo(pulseMid, pulseMin, 1, 1, 1);
    }
}

void delay_ms(uint32_t time_ms)
{
    volatile uint32_t i, j;
    for (i = 0; i < time_ms; i++)
        for (j = 0; j < 2000; j++)
            ;
}