#include <timer.h>
#include <interrupt.h>
#include <logger.h>
#include <utils.h>
#include <kmalloc.h>

#define CORE0_TIMER_IRQ_CTRL ((volatile uint32_t *)0x40000040)

timer_t *global_timer = (timer_t *)0x0;

MMIO_W_HELPER(_enable_timer_irq, CORE0_TIMER_IRQ_CTRL, 0x1, 1);

static inline
uint32_t _d(timer_t *t)
{ return t ? t->_d : 0; }

static inline
void _swap(timer_t **a, timer_t **b)
{
  timer_t *tmp = *a;
  *a = *b, *b = tmp;
}

static inline
timer_t *_merge(timer_t *a, timer_t *b)
{
  if (!a || !b)
    return a ?: b;
  
  if (a->data.firing_tick > b->data.firing_tick)
    _swap(&a, &b);
  a->_r = _merge(a->_r, b);

  if (_d(a->_l) < _d(a->_r))
    _swap(&a->_l, &a->_r);
  
  a->_d = _d(a->_r) + 1;
  return a;
}

static inline
timer_t *_pop(timer_t *t)
{
  if (!t || (!t->_l && !t->_r))
    return (timer_t *)0x0;
  return _merge(t->_l, t->_r);
}

static inline
void _set_timer_interrupt()
{
  if (global_timer) {
    uint64_t cntpct_el0;
    R_SYSREG(cntpct_el0, cntpct_el0);
    DEBUG_EXCEP(cntpct_el0);

    if (global_timer->data.firing_tick < cntpct_el0)
      W_SYSREG(cntp_tval_el0, 1);
    else
      W_SYSREG(cntp_cval_el0, global_timer->data.firing_tick);
    
    _enable_timer_irq(true);
  }
}

static inline
void _traverse(timer_t *t)
{
  if (!t) return;
  DEBUG_EXCEP(t->data.firing_tick);
  t->data.func(t->data.param);
  _traverse(t->_l);
  _traverse(t->_r);
}

void add_timer_task(task_t task)
{
  DEBUG_EXCEP("add timer task");
  timer_t *newtimer = kmalloc(sizeof(timer_t));
  *newtimer = (timer_t){
    ._l = (timer_t *)0x0,
    ._r = (timer_t *)0x0,
    ._d = 0,

    .data = task,
  };

  global_timer = _merge(global_timer, newtimer);

  _traverse(global_timer);
  _set_timer_interrupt();
}

void timer_irq_handler(void)
{
  _enable_timer_irq(false);

  LOG("timer irq");

  uint64_t cntpct_el0;
  R_SYSREG(cntpct_el0, cntpct_el0);
  DEBUG_EXCEP(cntpct_el0);

  if (global_timer) {
    add_interrupt_task(20, global_timer->data.func, global_timer->data.param);
    global_timer = _pop(global_timer);
  }

  _set_timer_interrupt();
}