#include "Clock_Sync.h"

#include "CAN_Communication.h"

#include "stm32g4xx_hal.h"
#include "stm32g4xx_hal_fdcan.h"
#include "stm32g4xx_hal_tim.h"

static FDCAN_HandleTypeDef* can;
static TIM_HandleTypeDef* sync_timer;
static TIM_HandleTypeDef* heartbeat_timer;

static uint8_t clock_sync_counter = 0;

void clock_sync_init(FDCAN_HandleTypeDef* can_handle,
                     TIM_HandleTypeDef* sync_timer_handle,
                     TIM_HandleTypeDef* heartbeat_timer_handle) {
  can = can_handle;
  sync_timer = sync_timer_handle;
  heartbeat_timer = heartbeat_timer_handle;
  HAL_TIM_Base_Start_IT(heartbeat_timer);
  // Delay between starting the timers so the interrupts don't fire at the same
  // time
  HAL_Delay(50);
  HAL_TIM_Base_Start_IT(sync_timer);
}

void clock_sync_handle_timer_complete(TIM_HandleTypeDef* timer) {
  if (timer == sync_timer) {
    CAN_Transmit(can, CLOCK_SYNC_ADDRESS, &clock_sync_counter, 1);
    clock_sync_counter++;
  } else if (timer == heartbeat_timer) {
    CAN_Transmit(can, MASTER_HEARTBEAT_ADDRESS, NULL, 0);
  }
}