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

#include "PowerManagement.h"
#include "UserInput.h"

int count = 0;
int test_func(int pid, unsigned int signal)
{
  M5.Lcd.fillScreen(TFT_DARKGREY);
  M5.Lcd.setCursor(0, 0);
  if (signal & SIGNAL_START)
  {
    M5.Lcd.setRotation(3);
    M5.Lcd.print("Initializing...");
  }
  if ((signal & SIGNAL_STOP)) // || count == 5)
  {
    M5.Lcd.print("Stopping");
    return 255;
  }
  if (signal & SIGNAL_TICK && (signal & SIGNAL_INPUT_A) == 0 && (signal & SIGNAL_INPUT_B) ==0)
  {
    count++;
    M5.Lcd.print("Ran: ");
    M5.Lcd.print(String(count));
    M5.Lcd.print(" times");
  }

  if (signal & SIGNAL_INPUT_A)
  {
    M5.Lcd.print("Input A!");
  }
  
  if (signal & SIGNAL_INPUT_B)
  {
    M5.Lcd.print("Input B!");
  }
  return 0;
}

void setup()
{
  M5.begin();
  Kernel_setup();
  //Kernel_start(&PowerManagement, 1000);
  Kernel_start(&UserInput, 0);
  Kernel_start(&test_func, 1000);
}

void loop()
{
  M5.update();
  Kernel_tick();
}