#ifndef DELAY_H
#define DELAY_H

#include <stdint.h>
#include <stm32h7xx.h>

#define SYSCLK_HZ ((uint32_t)64000000UL)
/* #define SYSCLK_HZ       ((uint32_t)480000000UL) */
#define D1CPRE_DIVIDER  ((uint32_t)1UL)
#define HCLK_DIVIDER    ((uint32_t)2UL)

#define D1CPRE_HZ       (SYSCLK_HZ / D1CPRE_DIVIDER)
#define HCLK_HZ         (D1CPRE_HZ / HCLK_DIVIDER)
#define SYSTICK_HZ      (D1CPRE_HZ / 8)

#define HCLK_FREQ_HZ    ((uint32_t)SYSTEM_CLOCK / AHB_DIVIDER / HCLK_DIVIDER)

inline void __attribute__((always_inline)) delay_us(uint32_t us) {
  uint64_t max_load = 0xFFFFFF;
  uint64_t total_load = us * (SYSTICK_HZ / 1000000);

  while (total_load > 0) {
    uint64_t curr_load = total_load;
    if (total_load > max_load) {
      curr_load = max_load;
    }
    SysTick->LOAD = curr_load;

    SysTick->VAL = 0;
    SysTick->CTRL = SysTick_CTRL_ENABLE_Msk; // Enable

    while (!(SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk));

    total_load -= curr_load;
    SysTick->CTRL = 0; // Disable
  }
}


inline void __attribute__((always_inline)) delay_ms(uint32_t ms) {
  for (uint32_t i = 0; i < ms; i++) {
    delay_us(1000);
  }
}

#endif // DELAY_H