#include "stm32g4xx.h"
#include "motors.h"
#include "pid.h"
#include "mixer.h"

void Motors_Init(void) {

    RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
    RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN;
    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;

    // PA0, PA1 -> TIM2 CH1/CH2
    GPIOA->MODER &= ~(GPIO_MODER_MODE0 | GPIO_MODER_MODE1);
    GPIOA->MODER |=  (GPIO_MODER_MODE0_1 | GPIO_MODER_MODE1_1);
    GPIOA->AFR[0] &= ~((0xF << 0) | (0xF << 4));
    GPIOA->AFR[0] |=  ((1 << 0) | (1 << 4));

    // PA10, PA11 -> TIM1 CH3/CH4
    GPIOA->MODER &= ~(GPIO_MODER_MODE10 | GPIO_MODER_MODE11);
    GPIOA->MODER |=  (GPIO_MODER_MODE10_1 | GPIO_MODER_MODE11_1);
    GPIOA->AFR[1] &= ~((0xF << 8) | (0xF << 12));
    GPIOA->AFR[1] |=  ((6 << 8) | (11 << 12));

    // TIM2 — 100 Гц
    TIM2->PSC = 16 - 1;
    TIM2->ARR = 10000 - 1;
    TIM2->CCMR1 = 0x6060;
    TIM2->CCER  = 0x11;
    TIM2->CR1   = TIM_CR1_CEN;

    // TIM1 — 100 Гц
    TIM1->PSC = 16 - 1;
    TIM1->ARR = 10000 - 1;
    TIM1->CCMR2 = 0x6060;
    TIM1->CCER  = 0x1100;
    TIM1->BDTR |= TIM_BDTR_MOE;
    TIM1->CR1   = TIM_CR1_CEN;
}

void Set_Motors(int val) {
    TIM2->CCR1 = val; // M1
    TIM2->CCR2 = val; // M2
    TIM1->CCR3 = val; // M3
    TIM1->CCR4 = val; // M4
}

void Set_Motor_Individual(int m1, int m2, int m3, int m4) {
    TIM2->CCR1 = m1;
    TIM2->CCR2 = m2;
    TIM1->CCR3 = m3;
    TIM1->CCR4 = m4;
}

void Motors_StartupDelay(void) {
    Set_Motors(900);
    for (volatile int i = 0; i < 6000000; i++) __NOP();
}

void Motors_Stop(void) {
    Set_Motors(900);
}

void Motors_Idle(void) {
    Set_Motors(1050);
}

void Motors_Arm(void) {
    Set_Motors(1050);
}

void Motors_Disarm(void) {
    Set_Motors(900);
}