template <uint_t i_timer>
class CTimerImpl
{
private:
static const AT91PS_TC m_timer;
static const uint_t m_timer_id =
i_timer == 0 ? AT91C_ID_TC0 :
i_timer == 1 ? AT91C_ID_TC1 :
i_timer == 2 ? AT91C_ID_TC2 : 0;

private:
static const uint32_t m_max_ticks = 0x10000;
static const uint32_t m_interrupt_priority = 1;

public:
static CTimerImpl<i_timer>* m_instance;

private:
float m_max_time_ms;
float m_ticks_per_ms;

private:
static void interrupt_handler()
{
uint32_t mask = m_timer->TC_SR;
uint32_t x = 0;
x = mask;
// m_instance->on_interrupt(mask);
}

inline bool on_interrupt(uint32_t mask)
{
if (mask & AT91C_TC_CPAS)
on_event_a();
if (mask & AT91C_TC_CPBS)
on_event_b();
if (mask & AT91C_TC_CPCS)
on_overflow();

return (mask & (AT91C_TC_CPAS | AT91C_TC_CPCS | AT91C_TC_CPBS)) != 0;
}

uint32_t calculate_optimal_divider(float max_period_ms)
{
const float cycles_per_ms = CSystem::get_instance()->get_frequency() / 1000.0f;
const float max_time_ms = m_max_ticks / cycles_per_ms;

m_max_time_ms = max_time_ms * 2.0f;
m_ticks_per_ms = cycles_per_ms / 2.0f;

if (m_max_time_ms > max_period_ms)
return AT91C_TC_CLKS_TIMER_DIV1_CLOCK;

m_max_time_ms = max_time_ms * 8.0f;
m_ticks_per_ms = cycles_per_ms / 8.0f;

if (m_max_time_ms > max_period_ms)
return AT91C_TC_CLKS_TIMER_DIV2_CLOCK;

m_max_time_ms = max_time_ms * 32.0f;
m_ticks_per_ms = cycles_per_ms / 32.0f;

if (m_max_time_ms > max_period_ms)
return AT91C_TC_CLKS_TIMER_DIV3_CLOCK;

m_max_time_ms = max_time_ms * 128.0f;
m_ticks_per_ms = cycles_per_ms / 128.0f;

if (m_max_time_ms > max_period_ms)
return AT91C_TC_CLKS_TIMER_DIV4_CLOCK;

m_max_time_ms = max_time_ms * 1024.0f;
m_ticks_per_ms = cycles_per_ms / 1024.0f;

if (m_max_time_ms > max_period_ms)
return AT91C_TC_CLKS_TIMER_DIV5_CLOCK;

__STL_THROW(invalid_argument("vary big period"));
return AT91C_TC_CLKS_TIMER_DIV5_CLOCK;
}

protected:
virtual void on_event_a() = 0;
virtual void on_event_b() = 0;
virtual void on_overflow() = 0;

void initialize(float max_time_ms)
{
m_instance = this;

AT91F_AIC_CfgPMC();
AT91F_PMC_EnablePeriphClock(AT91C_BASE_PMC, 1 << m_timer_id);

// Disable TC clock
m_timer->TC_CCR = AT91C_TC_CLKDIS;
AT91F_TC_InterruptDisable(m_timer, 0xFFFFFFFF);

// Clear status register
uint32_t mask = m_timer->TC_SR;
mask = ~mask;

// Set mode
m_timer->TC_CMR = calculate_optimal_divider(max_time_ms) | AT91C_TC_WAVESEL_UP_AUTO | AT91C_TC_WAVE | AT91C_TC_EEVT_XC0;

// enable interrupts
AT91F_TC_InterruptEnable(m_timer, AT91C_TC_CPCS | AT91C_TC_CPAS | AT91C_TC_CPBS);

// configure interrupt handler
AT91F_AIC_ConfigureIt(
AT91C_BASE_AIC,
m_timer_id,
m_interrupt_priority,
AT91C_AIC_SRCTYPE_INT_HIGH_LEVEL,
(void(*)())interrupt_handler
);

// enable interrupt
AT91F_AIC_EnableIt(AT91C_BASE_AIC, m_timer_id);
}

void deinitialize()
{
stop();

// disable interrupts
AT91F_TC_InterruptDisable(m_timer, AT91C_TC_CPCS | AT91C_TC_CPAS | AT91C_TC_CPBS);

// mode
m_timer->TC_CMR = 0;

// perepheral clock
AT91F_PMC_DisablePeriphClock(AT91C_BASE_PMC, 1 << m_timer_id);
}

void set_time_a(float time_a_ms)
{
m_timer->TC_RA = (uint32_t)(std_math::min(time_a_ms, m_max_time_ms) * m_ticks_per_ms);
}

void set_time_b(float time_b_ms)
{
m_timer->TC_RB = (uint32_t)(std_math::min(time_b_ms, m_max_time_ms) * m_ticks_per_ms);
}

void set_time_reset(float period_ms)
{
m_timer->TC_RC = (uint32_t)(std_math::min(period_ms, m_max_time_ms) * m_ticks_per_ms);
}

inline void start()
{
m_timer->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
}

inline void stop()
{
m_timer->TC_CCR = AT91C_TC_CLKDIS;
}
};

template <>
const AT91PS_TC CTimerImpl<0>::m_timer = AT91C_BASE_TC0;

template <>
const AT91PS_TC CTimerImpl<1>::m_timer = AT91C_BASE_TC1;

template <>
const AT91PS_TC CTimerImpl<2>::m_timer = AT91C_BASE_TC2;

template <uint_t i_timer>
CTimerImpl<i_timer>* CTimerImpl<i_timer>::m_instance;

    CTimer<0> timer(10.0f);

    timer.set_time_a(2.0f);
    timer.set_time_b(6.0f);
    timer.set_time_reset(8.0f);

    timer.start();

    while(1);

void timer_handler()
{
  unsigned int mask = AT91C_BASE_TC0->TC_SR;
  mask ++;
}

int main()
{
  // enable PMC
  AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_FIQ;
  AT91C_BASE_PMC->PMC_PCER = 1 << AT91C_ID_TC0;

  // set mode
  AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
  AT91C_BASE_TC0->TC_IDR = 0xFFFFFFFF;
  AT91C_BASE_TC0->TC_SR;
  AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_TIMER_DIV5_CLOCK | AT91C_TC_WAVESEL_UP_AUTO | AT91C_TC_WAVE | AT91C_TC_EEVT_XC0;

  // enable interrupts on compare A, B, C
  AT91C_BASE_TC0->TC_IER = AT91C_TC_CPAS | AT91C_TC_CPBS | AT91C_TC_CPCS;
  AT91C_BASE_TC0->TC_RA = 100;
  AT91C_BASE_TC0->TC_RB = 200;
  AT91C_BASE_TC0->TC_RC = 400;

  // start
  AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;

  // set irq handler
  AT91C_BASE_AIC->AIC_IDCR = 1 << AT91C_ID_TC0;
  AT91C_BASE_AIC->AIC_SMR[AT91C_ID_TC0] = 0;
  AT91C_BASE_AIC->AIC_SVR[AT91C_ID_TC0] = (unsigned int)timer_handler;
  AT91C_BASE_AIC->AIC_ICCR = 1 << AT91C_ID_TC0;

  // enable interrupts from timer
  AT91C_BASE_AIC->AIC_IECR = 1 << AT91C_ID_TC0;

  while (1);
}

volatile static int ss;

void timer_handler()
{
  unsigned int mask = AT91C_BASE_TC0->TC_SR;
  ss = (mask & (AT91C_TC_CPAS | AT91C_TC_CPBS | AT91C_TC_CPCS)) == (AT91C_TC_CPAS | AT91C_TC_CPBS);
}

  while (1)
  {
    printf("%d", ss);
  }

  while (true)
  {
    for (uint32_t x = 0; x < 20000; x ++)
    {
      servo0.set_position(servo::min_position + x * (servo::max_position - servo::min_position) / 20000.0f);
      servo2.set_position(servo::min_position + x * (servo::max_position - servo::min_position) / 20000.0f);
      // ...
      servo11.set_position(servo::min_position + x * (servo::max_position - servo::min_position) / 20000.0f);
    }

    for (uint32_t x = 0; x < 20000; x ++)
    {
      servo0.set_position(servo::max_position - x * (servo::max_position - servo::min_position) / 20000.0f);
      servo2.set_position(servo::max_position - x * (servo::max_position - servo::min_position) / 20000.0f);
      // ...
      servo11.set_position(servo::max_position - x * (servo::max_position - servo::min_position) / 20000.0f);
    }
  }

  while (true)
  {
    start_time = watch->get_time_us();
    
    while (true)
    {
      uint64_t current_time = watch->get_time_us();
  
      if (current_time - start_time > 5000000)
        break;
  
      float x = (current_time - start_time) / 5000000.0f;
      servo0.set_position(servo::min_position + x * (servo::max_position - servo::min_position));
    }

    start_time = watch->get_time_us();
    
    while (true)
    {
      uint64_t current_time = watch->get_time_us();
  
      if (current_time - start_time > 5000000)
        break;
  
      float x = (current_time - start_time) / 5000000.0f;
      servo0.set_position(servo::max_position - x * (servo::max_position - servo::min_position));
    }
  }

AT91S_AIC  *pAIC = AT91C_BASE_AIC;
void INIT_RTTC (int Alarm_Value_seconds) {                       /* Initialize Real Time Timer */
   pRTTC->RTTC_RTAR = (Alarm_Value_seconds - 1);
   pAIC->AIC_IECR = AT91C_ID_SYS;
   pAIC->AIC_SMR[1] = AT91C_AIC_PRIOR_HIGHEST | AT91C_AIC_SRCTYPE_POSITIVE_EDGE;
   pRTTC->RTTC_RTMR = (AT91C_RTTC_RTPRES & 0x8000) | AT91C_RTTC_ALMIEN | AT91C_RTTC_RTTRST;
}

__irq void RTTC_Interrupt_Handler (void) {

        if ((pRTTC->RTTC_RTSR & AT91C_RTTC_ALMS) != 0) {
              LED_ON;
        } else {
              LED_OFF;
        }

}

Reset_Addr      DCD     Reset_Handler
Undef_Addr      DCD     Undef_Handler
SWI_Addr        DCD     SWI_Handler
PAbt_Addr       DCD     PAbt_Handler
DAbt_Addr       DCD     DAbt_Handler
                DCD     0        ; Reserved Address
IRQ_Addr        DCD     IRQ_Handler
FIQ_Addr        DCD     FIQ_Handler

Undef_Handler   B       Undef_Handler
SWI_Handler     B       SWI_Handler
PAbt_Handler    B       PAbt_Handler
DAbt_Handler    B       DAbt_Handler
                   PRESERVE8
                   IMPORT RTTC_Interrupt_Handler
IRQ_Handler     B       RTTC_Interrupt_Handler
FIQ_Handler     B       FIQ_Handler