#include <M5StickC.h>
#include "Kernel.h"
#include "Util.h"
#include "Colours.h"

#define MAX_TASKS 8

struct Task *Kernel_get_task(int pid);

struct Task tasks[MAX_TASKS];
int next_pid = 1;
unsigned long last_tick;

void Kernel_panic(char* message)
{
  M5.Lcd.setRotation(3);
  M5.Lcd.fillScreen(TFT_BLUE);
  M5.Lcd.setTextColor(TFT_WHITE, TFT_BLUE);
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.print("Panic! ");
  M5.Lcd.print(message);
  while(true) {}
}

void Kernel_setup()
{
  last_tick = millis();
}

void Kernel_start(int (*callback)(unsigned int), unsigned long interval)
{
  struct Task *process = Kernel_get_task(0);
  if (process->pid == -1)
  {
    Kernel_panic("Could not start process -- reached MAX_TASKS");
  }
  
  process->pid = next_pid++;
  process->callback = callback;
  process->run_interval = interval;
  process->run_accumulator = interval;
  process->signal = SIGNAL_START | SIGNAL_TICK;
  process->running = true;
}

void Kernel_enable(int pid)
{
  struct Task *process = Kernel_get_task(pid);
  if (process->pid == pid)
  {
    process->running = true;
  }
}

void Kernel_disable(int pid)
{
  struct Task *process = Kernel_get_task(pid);
  if (process->pid == pid)
  {
    process->running = false;
  }
}

void Kernel_reap(int pid)
{
  struct Task *process = Kernel_get_task(pid);
  if (process->pid != pid || process->running)
  {
    return;
  }
  process->pid = 0;
  process->running = false;
  process->exit_code = 0;
  process->run_interval = 0;
  process->run_accumulator = 0;
  process->signal = SIGNAL_NONE;
  process->signal_mask = 0xFFFF;
}

int Kernel_get_exit_code(int pid)
{
  struct Task *process = Kernel_get_task(pid);
  int exit_code = process->pid != pid || process->running == true ? -1 : process->exit_code;
  Kernel_reap(pid);
  return exit_code;
}

bool Kernel_is_exited(int pid)
{
  struct Task *process = Kernel_get_task(pid);
  return process->pid != pid || process->running == false;
}

struct Task *Kernel_get_task(int pid)
{
  for (int i=0; i<MAX_TASKS; i++)
  {
    if (tasks[i].pid == pid)
    {
      return &tasks[i];
    }
  }
  struct Task empty = Task();
  empty.pid = -1;
  return &empty;
}

void Kernel_tick()
{
  unsigned long duration = millis_since(last_tick);
  // Store the last runtime so we can calculate duration next tick. We do this at the beginning of the method
  // so it counts the time spent running tasks as time elapsed.
  last_tick = millis();

  unsigned long next_tick_due = ULONG_MAX;
  
  for (int i=0; i<MAX_TASKS; i++)
  {
    // If the task is initialized (non-zero PID) and is marked as running
    if (tasks[i].pid != 0 && tasks[i].running == true)
    {

      // Check if this tick's duration will push us over the run interval and if so set the TICK signal
      if (tasks[i].run_accumulator + duration > tasks[i].run_interval)
      {
        // Set signal
        tasks[i].signal |= SIGNAL_TICK;
        // Reset accumulator so we can start counting again
        tasks[i].run_accumulator = 0;
      }
      else
      {
        // Otherwise, accumulate time
        tasks[i].run_accumulator += duration;
      }

      // If the process has any non-masked signals pending, run it
      if ((tasks[i].signal & tasks[i].signal_mask) != 0)
      {
        // Run the task
        int task_return = (*tasks[i].callback)(tasks[i].signal);
        // Reset the signal
        tasks[i].signal = SIGNAL_NONE;

        // If the tasks's return value was non-zero, it has exited.
        if (task_return != 0)
        {
          // Mark it as no longer running
          tasks[i].running = false;
          // Store the exit code
          tasks[i].exit_code = task_return;
        }
      }
      

      // Check each task to see if it's the one scheduled to run on the next closest tick, and track it
      // so we can put the processor to sleep until then.
      if (tasks[i].run_interval - tasks[i].run_accumulator < next_tick_due)
      {
        next_tick_due = tasks[i].run_interval - tasks[i].run_accumulator;
      }
    }
  }

  // Put processor to sleep until the next scheduled run of a task
  // Leave the home button enabled to force a wake-up before then
  esp_sleep_enable_timer_wakeup(next_tick_due * 1000);
  esp_sleep_enable_ext0_wakeup((gpio_num_t)37, LOW);
  esp_light_sleep_start();
}