#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_start;
unsigned long last_tick_duration;
bool inputs[2] = {false, false};

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_start = millis();
  last_tick_duration = 0;
}

int Kernel_start(int (*callback)(int, 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;

  return process->pid;
}

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;
}

void Kernel_signal(unsigned int signal)
{
  for (int i=0; i<MAX_TASKS; i++)
  {
    if (tasks[i].pid != 0 && tasks[i].running == true)
    {
      tasks[i].signal_pending |= signal;
    }
  }
}

void Kernel_signal(int pid, unsigned int signal)
{
  struct Task *process = Kernel_get_task(pid);
  if (process->pid == pid)
  {
    process->signal_pending |= signal;
  }
}

void Kernel_signal_mask(int pid, unsigned int signal_mask)
{
  struct Task *process = Kernel_get_task(pid);
  if (process->pid == pid)
  {
    process->signal_mask = signal_mask;
  }
}

unsigned long Kernel_get_run_interval(int pid)
{
  struct Task *process = Kernel_get_task(pid);
  return process->pid == pid ? process->run_interval : 0;
}

void Kernel_set_run_interval(int pid, unsigned long interval)
{
  struct Task *process = Kernel_get_task(pid);
  if (process->pid == pid)
  {
    process->run_interval = interval;
  }
  
}

int Kernel_count_running_tasks()
{
  int count = 0;
  for (int i=0; i<MAX_TASKS; i++)
  {
    if (tasks[i].pid != 0 && tasks[i].running)
    {
      count++;
    }
  }
  return count;
}

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;
}

bool Kernel_read_input(int input)
{
  if (inputs[input] == true && ((input == 0 && digitalRead(BUTTON_A_PIN) == 1) || (input == 1 && digitalRead(BUTTON_B_PIN) == 1)))
  {
    inputs[input] = false;
  }
  return inputs[input];
}

unsigned long Kernel_get_last_tick_duration()
{
  return last_tick_duration;
}

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()
{
  last_tick_duration = millis_since(last_tick_start);
  // 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_start = millis();

  unsigned long next_tick_due = ULONG_MAX;
  int running_tasks = 0;
  int last_exit_code = 0;

  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)
    {
      // Make sure we found at least one active process, because if not we dead.
      running_tasks++;

      // Check if this process is running on a schedule and this tick's duration will push us over the run interval and if so set the TICK signal
      // run_interval 0 is a special case of "run whenever we run", so set TICK any time we get here
      if (tasks[i].run_interval > 0)
      {
        if (tasks[i].run_accumulator + last_tick_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 += last_tick_duration;
        }
      }
      else
      {
        tasks[i].signal |= SIGNAL_TICK;
      }

      // 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].pid, tasks[i].signal);
        
        // 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;
          // Actually it's not running...
          running_tasks--;
          last_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].running && tasks[i].run_interval > 0 && tasks[i].run_interval - tasks[i].run_accumulator < next_tick_due)
      {
        next_tick_due = tasks[i].run_interval - tasks[i].run_accumulator;
      }
    }
  }

  if (running_tasks == 0)
  {
    char panic_msg[255];
    sprintf(panic_msg, "All processes exited! Last exit code: %d", last_exit_code);
    Kernel_panic(panic_msg);
  }

  bool signals_pending = false;
  for (int i=0; i<MAX_TASKS; i++)
  {
    tasks[i].signal = tasks[i].signal_pending;
    tasks[i].signal_pending = SIGNAL_NONE;
    
    if ((tasks[i].signal & tasks[i].signal_mask) != 0)
    {
      signals_pending = true;
    }
  }
  
  // If no signals pending, put processor to sleep until the next scheduled run of a task
  if (!signals_pending)
  {
    esp_sleep_enable_timer_wakeup(next_tick_due * 1000);
    esp_sleep_enable_ext0_wakeup((gpio_num_t)37, LOW);
    esp_sleep_enable_ext1_wakeup(0x8000000000, ESP_EXT1_WAKEUP_ALL_LOW);
    esp_light_sleep_start();

    esp_sleep_wakeup_cause_t wakeup_reason;
    wakeup_reason = esp_sleep_get_wakeup_cause();
    switch(wakeup_reason){
      case ESP_SLEEP_WAKEUP_TIMER     : break;
      case ESP_SLEEP_WAKEUP_EXT0      : inputs[0] = true; break;
      case ESP_SLEEP_WAKEUP_EXT1      : inputs[1] = true; break;
      case ESP_SLEEP_WAKEUP_TOUCHPAD  : break;
      case ESP_SLEEP_WAKEUP_ULP       : break;
      case ESP_SLEEP_WAKEUP_GPIO      : break;
      case ESP_SLEEP_WAKEUP_UART      : break;
      default                         : break;
    }
  }

  if (digitalRead(BUTTON_A_PIN) != LOW) inputs[0] = false;
  if (digitalRead(BUTTON_B_PIN) != LOW) inputs[1] = false;
  
}