#include "TimeSync.h"

#include "stm32f412rx.h"
#include "stm32f4xx_hal.h"

#include <stdint.h>

void time_sync_handle_frame() {
  static uint32_t last_time_sync_frame = 0;
  static uint32_t f_pre_trim = TARGET_FREQ;
  static uint8_t trimmed_last_frame = 0;
  static int32_t last_trim_delta = TRIM_FREQ;

  uint32_t now = HAL_GetTick();
  if (last_time_sync_frame != 0) {
    uint32_t n_measured = now - last_time_sync_frame;
    uint32_t f_real = n_measured * (TARGET_FREQ / TIME_SYNC_INTERVAL);
    if (trimmed_last_frame) {
      last_trim_delta = (f_pre_trim - f_real) / trimmed_last_frame;
      if (last_trim_delta < 0) {
        last_trim_delta = -last_trim_delta;
      }
      trimmed_last_frame = 0;
    }

    int32_t delta_f = TARGET_FREQ - f_real;
    int32_t delta_quants = delta_f / last_trim_delta;
    if (delta_quants != 0) {
      uint32_t trim_delta = (now < STARTUP_TIME_SYNC_TIME) ? 2 : 1;
      uint32_t rcc_cr = RCC->CR;
      // Determine current trim
      int32_t trim = (rcc_cr & RCC_CR_HSITRIM_Msk) >> RCC_CR_HSITRIM_Pos;
      if (delta_quants > 0) {
        trim += trim_delta;
        if (trim > RCC_CR_HSITRIM_MAX) {
          trim = RCC_CR_HSITRIM_MAX;
        }
      } else if (delta_quants < 0) {
        trim -= trim_delta;
        if (trim < 0) {
          trim = 0;
        }
      }
      // Clear current trim and overwrite with new trim
      rcc_cr = (rcc_cr & ~RCC_CR_HSITRIM_Msk) |
               ((trim << RCC_CR_HSITRIM_Pos) & RCC_CR_HSITRIM_Msk);
      RCC->CR = rcc_cr;
      f_pre_trim = f_real;
      trimmed_last_frame = trim_delta;
    }
  }
  last_time_sync_frame = now;
}