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

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

interrupt_t *global_interrupt_pool = (interrupt_t *)0x0;

void enable_interrupt(void)
{ W_SYSREG_IMM(DAIFClr, 0xf); }

void disable_interrupt(void)
{ W_SYSREG_IMM(DAIFSet, 0xf); }

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

MMIO_R_HELPER(_irq_source_timer, CORE0_IRQ_SOURCE, 0x1, 1);
MMIO_R_HELPER(_irq_source_uart,  CORE0_IRQ_SOURCE, 0x1, 8);

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

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

static inline
interrupt_t *_merge(interrupt_t *a, interrupt_t *b)
{
  if (!a || !b)
    return a ?: b;

  if (a->priority < b->priority)
    _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
interrupt_t *_pop(interrupt_t *interrupt)
{
  if (!interrupt || (!interrupt->_l && !interrupt->_r))
    return (interrupt_t *)0x0;
  return _merge(interrupt->_l, interrupt->_r);
}

void add_interrupt_task(uint64_t priority,
                        interrupt_callback_func_t func,
                        uint64_t param)
{
  // DEBUG_EXCEP("add interrupt");
  uint64_t last_priority = 0xfff;
  if (global_interrupt_pool)
    last_priority = global_interrupt_pool->priority;
  interrupt_t *newint = kmalloc(sizeof(interrupt_t));
  *newint = (interrupt_t){
    ._l = (interrupt_t *)0x0,
    ._r = (interrupt_t *)0x0,
    ._d = 0,

    .priority = priority,
    .func = func,
    .param = param,

    .is_start = false,
  };

  global_interrupt_pool = _merge(global_interrupt_pool, newint);

  while (global_interrupt_pool) {
    // LOG("check interrupt"); DEBUG_EXCEP(global_interrupt_pool->priority);
    if (global_interrupt_pool->is_start && global_interrupt_pool->priority < last_priority)
      return;
    global_interrupt_pool->is_start = true;
    global_interrupt_pool->func(global_interrupt_pool->param);
    global_interrupt_pool = _pop(global_interrupt_pool);
  }
}

void irq_handler(void)
{
  // uint64_t core0_irq_source = *CORE0_IRQ_SOURCE;
  // DEBUG_EXCEP(core0_irq_source);

  if (_irq_source_timer())
    timer_irq_handler();

  if (_irq_source_uart())
    uart_irq_handler();
}

void wfe(void)
{
  return; // do nothing for now

  if (!global_interrupt_pool) {
    // asm volatile("wfe");
    asm volatile("nop");
    return;
  }

  interrupt_t *interrupt = global_interrupt_pool;
  global_interrupt_pool = _merge(global_interrupt_pool->_l,
                                 global_interrupt_pool->_r);

  interrupt->func(interrupt->param);
}