#include "scheduler.h"
#include <stdlib.h>

void task_a(void) {
  while (true) printk("A");
}

void task_b(void) {
  while (true) printk("B");
}

constexpr uint16_t max_tasks = 256;
uint16_t curr_task = 0;
uint16_t last_task = 0;
static Scheduler::task tasks[max_tasks];

/* TODO dynamically allocate task stacks */
__attribute__((aligned(16))) uint8_t stack_a[4096] = {0};
__attribute__((aligned(16))) uint8_t stack_b[4096] = {0};

Scheduler::Scheduler(uint16_t cs) : InterruptHandler(0x00) {
  printk("add_task: ", add_task({stack_a, task_a, cs}), " stack ",
         uhex{reinterpret_cast<uint32_t>(tasks[1].cpu) + sizeof(cpu_state_t)}, " eip: ", uhex{tasks[1].cpu->eip}, '\n');
  printk("add_task: ", add_task({stack_b, task_b, cs}), " stack ",
         uhex{reinterpret_cast<uint32_t>(tasks[2].cpu) + sizeof(cpu_state_t)}, " eip: ", uhex{tasks[2].cpu->eip}, '\n');
}

uint16_t Scheduler::add_task(Scheduler::task t) {
  if (last_task == (max_tasks - 1)) return 0;

  uint16_t id = ++last_task;
  tasks[id] = t;

  return id;
}

cpu_state_t* Scheduler::trigger(cpu_state_t* cpu) {
  if (curr_task != 0) {
    tasks[curr_task].cpu = cpu;
  }
  ++curr_task;
  if (curr_task > last_task) curr_task = 1;
  cpu = tasks[curr_task].cpu;
  return cpu;
}