#include "ltimer.h"

TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim4;

void ltimer_init(void)
{
    __HAL_RCC_TIM2_CLK_ENABLE();
    htim2.Instance = TIM2;
    htim2.Init.Prescaler = (SystemD2Clock / 1000000) - 1; // 1us
    htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim2.Init.Period = 0xFFFFFFFF;
    htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
    htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    HAL_TIM_Base_Init(&htim2);

    __HAL_RCC_TIM4_CLK_ENABLE();
    htim4.Instance = TIM4;
    htim4.Init.Prescaler = 0;
    htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
    htim4.Init.Period = 0xFFFFFFFF;
    htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
    HAL_TIM_Base_Init(&htim4);

    TIM_MasterConfigTypeDef sMasterConfig = {0};
    sMasterConfig.MasterOutputTrigger = TIM_TRGO_UPDATE;
    sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_ENABLE;
    HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig);

    TIM_SlaveConfigTypeDef sSlaveConfig = {0};
    sSlaveConfig.SlaveMode = TIM_SLAVEMODE_EXTERNAL1;
    sSlaveConfig.InputTrigger = TIM_TS_ITR1;
    HAL_TIM_SlaveConfigSynchro(&htim4, &sSlaveConfig);

    HAL_TIM_Base_Start(&htim2);
    HAL_TIM_Base_Start(&htim4);
}

uint64_t ltimer_get_us(void)
{
    uint32_t tim2_count;
    uint32_t tim4_count_0, tim4_count_1;
    uint64_t us;

    do {
        tim4_count_0 = __HAL_TIM_GET_COUNTER(&htim4);
        tim2_count = __HAL_TIM_GET_COUNTER(&htim2);
        tim4_count_1 = __HAL_TIM_GET_COUNTER(&htim4);
    } while (tim4_count_0 != tim4_count_1);

    us = (uint64_t)tim2_count | ((uint64_t)tim4_count_1 << 32);

    return us;
}

uint64_t ltimer_get_ms(void)
{
    return (ltimer_get_us() + 500) / 1000;
}

void ltimer_delay_us(uint32_t us)
{
    uint64_t start;

    start = ltimer_get_us();
    while (ltimer_get_us() - start < us) {

    }
}

void ltimer_delay_ms(uint32_t ms)
{
    uint64_t start;

    start = ltimer_get_us();
    while (ltimer_get_us() - start < ms * 1000) {

    }
}