#include "stm32g4xx.h"
#include "imu.h"
#include "motors.h"
#include "dsp_manager.h"
#include "dshot2.h"

// 1. ПЕРЕМЕННЫЕ
volatile uint8_t imu_flag = 0;

uint32_t m1_speed = 0;
uint32_t m2_speed = 0;
uint32_t m3_speed = 0;
uint32_t m4_speed = 0;

#pragma pack(push, 1)
typedef struct {
    uint8_t  header[2];
    int16_t  gx;
    int16_t  filt_gx;
    uint16_t notch1_hz;
    uint16_t notch2_hz;
    uint16_t notch3_hz;
} Telemetry_t;
#pragma pack(pop)

// 2. ПРОТОТИПЫ
void SystemClock_Config_160MHz(void);
void UART2_Init_921600(void);
void TIM7_Init_1000Hz(void);
void UART_SendPacket(Telemetry_t *p);

// 3. ОСНОВНОЙ ЦИКЛ
int main(void) {
    SystemClock_Config_160MHz();
    Motors_Init();
    Set_Motors(900);
    for (int i = 0; i < 20000000; i++) __NOP(); // Пауза для ESC

    I2C1_Init();
    UART2_Init_921600();
    IMU_Init();
    IMU_Calibrate();
    
    DSP_Init(); // Инициализация нашего анализатора Фурье
    
    TIM7_Init_1000Hz();
    __enable_irq();

    Telemetry_t pkt;
    pkt.header[0] = 0xAA; pkt.header[1] = 0xBB;

    while (1) {
        if (imu_flag) {
              imu_flag = 0;
              
              static unsigned short dshot_erpm[4];
              DSHOT_GetERPM(dshot_erpm, 0);
              DSP_SetMotorErpm(dshot_erpm, 7);

              IMU_ReadRawData(); 
              //Motors_PollTelemetry();

              //сохраняем во временную переменную
              int16_t gx_val = raw_gx; 
              DSP_AddSample((float32_t)gx_val);

              if (fft_ready) {
                  DSP_Process(); 
              }

              float32_t filt_gx_value = filt_gx;
              pkt.gx = gx_val;
              pkt.filt_gx = (int16_t)filt_gx_value;
              pkt.notch1_hz = notch_report_hz[0];
              pkt.notch2_hz = notch_report_hz[1];
              pkt.notch3_hz = notch_report_hz[2];
              UART_SendPacket(&pkt);

              
              uint32_t m1 = m1_speed;
              uint32_t m2 = m2_speed;
              uint32_t m3 = m3_speed;
              uint32_t m4 = m4_speed;
              Set_Motor_Individual(m1, m2, m3, m4);
          }
        }
    }

// 4. РЕАЛИЗАЦИЯ ФУНКЦИЙ

void SystemClock_Config_160MHz(void) {
    RCC->CR |= RCC_CR_HSION;
    while(!(RCC->CR & RCC_CR_HSIRDY));
    RCC->PLLCFGR = (RCC_PLLCFGR_PLLSRC_HSI | (3 << RCC_PLLCFGR_PLLM_Pos) | 
                   (80 << RCC_PLLCFGR_PLLN_Pos) | (0 << RCC_PLLCFGR_PLLR_Pos));
    RCC->CR |= RCC_CR_PLLON;
    RCC->PLLCFGR |= RCC_PLLCFGR_PLLREN;
    while(!(RCC->CR & RCC_CR_PLLRDY));
    FLASH->ACR |= (4 << FLASH_ACR_LATENCY_Pos);
    RCC->CFGR |= RCC_CFGR_SW_PLL;
    while((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL);
    SystemCoreClockUpdate();
}

void UART2_Init_921600(void) {
    RCC->AHB2ENR  |= RCC_AHB2ENR_GPIOBEN;
    RCC->APB1ENR1 |= RCC_APB1ENR1_USART2EN;
    RCC->CCIPR    |= (1 << RCC_CCIPR_USART2SEL_Pos);
    GPIOB->MODER &= ~(GPIO_MODER_MODE3 | GPIO_MODER_MODE4);
    GPIOB->MODER |=  (GPIO_MODER_MODE3_1 | GPIO_MODER_MODE4_1);
    GPIOB->AFR[0] &= ~((0xF << 12) | (0xF << 16));
    GPIOB->AFR[0] |=  ((7 << 12) | (7 << 16));
    USART2->BRR = 174; 
    USART2->CR1 = USART_CR1_TE | USART_CR1_UE;
}

void UART_SendPacket(Telemetry_t *p) {
    uint8_t *ptr = (uint8_t*)p;
    for (uint16_t i = 0; i < sizeof(Telemetry_t); i++) {
        while (!(USART2->ISR & USART_ISR_TXE));
        USART2->TDR = ptr[i];
    }
}

void TIM7_Init_1000Hz(void) {
    RCC->APB1ENR1 |= RCC_APB1ENR1_TIM7EN;
    TIM7->PSC = 16000 - 1; 
    TIM7->ARR = 10 - 1;
    TIM7->DIER |= TIM_DIER_UIE;
    NVIC_EnableIRQ(TIM7_DAC_IRQn);
    TIM7->CR1 |= TIM_CR1_CEN;
}

extern "C" void TIM7_DAC_IRQHandler(void) {
    if (TIM7->SR & TIM_SR_UIF) {
        TIM7->SR = 0;
        imu_flag = 1;
    }
}