#include "ClockSync.h"

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

#include <stdint.h>

static uint32_t last_clock_sync_frame = 0;

void clock_sync_handle_frame() {
  static uint32_t f_pre_trim = CLOCK_TARGET_FREQ;
  static int32_t trimmed_last_frame = 0;
  static int32_t last_trim_delta = HSI_TRIM_FREQ;

  uint32_t now = HAL_GetTick();
  if (last_clock_sync_frame != 0) {
    uint32_t n_measured = now - last_clock_sync_frame;
    uint32_t f_real = n_measured * (CLOCK_TARGET_FREQ / CLOCK_SYNC_INTERVAL);
    if (trimmed_last_frame) {
      last_trim_delta = ((int32_t)(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 = CLOCK_TARGET_FREQ - f_real;
    int32_t delta_quants = delta_f / last_trim_delta;
    if (delta_quants != 0) {
      int32_t trim_delta = (now < STARTUP_CLOCK_SYNC_TIME) ? 2 : 1;
      if (delta_quants < 0) {
        trim_delta = -trim_delta;
      }
      trim_hsi(trim_delta);
      f_pre_trim = f_real;
      trimmed_last_frame = trim_delta;
    }
  }
  last_clock_sync_frame = now;
}

void clock_sync_startup_check() {
  static uint32_t last_startup_trim = 0;

  uint32_t now = HAL_GetTick();
  if (now - last_clock_sync_frame > CLOCK_SYNC_INTERVAL &&
      now - last_startup_trim > CLOCK_SYNC_INTERVAL) {
    // The slave is probably warm, which increases the clock frequency. If the
    // frequency is too high, we won't receive any CAN frames anymore, so let's
    // try trimming down a bit.
    trim_hsi(-2);
    last_startup_trim = now;
  }
}

void trim_hsi(int32_t delta) {
  uint32_t rcc_cr = RCC->CR;
  // Determine current trim
  int32_t trim = (rcc_cr & RCC_CR_HSITRIM_Msk) >> RCC_CR_HSITRIM_Pos;
  trim += delta;
  if (trim > RCC_CR_HSITRIM_MAX) {
    trim = RCC_CR_HSITRIM_MAX;
  } else 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;
}