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

#include "UI.h"
#include "PowerManagement.h"
#include "UserInput.h"
#include "Clock.h"
#include "Battery.h"

int count = 0;
bool input_a = false, input_b = false;

void test_draw(int hwnd, int x, int y, int width, int height)
{
  M5.Lcd.setCursor(x, y);
  M5.Lcd.println(String(count));
  y += 10;
  if (input_a)
  {
    M5.Lcd.setCursor(x, y);
    M5.Lcd.println("Input A!");
    y += 10;
  }
  if (input_b)
  {
    M5.Lcd.setCursor(x, y);
    M5.Lcd.println("Input B!");
    y += 10;
  }
}

int test_func(int pid, unsigned int signal)
{ 
  if (signal & SIGNAL_START)
  {
    Kernel_signal_mask(pid, SIGNAL_START | SIGNAL_STOP | SIGNAL_TICK | SIGNAL_INPUT_A | SIGNAL_INPUT_B);
    if (EAT_allocate(1, 2))
    {
      EAT_write(1, 0, 0);
      EAT_write(1, 1, 0);
    }
    else
    {
      byte count_low = EAT_read(1, 0);
      byte count_hi = EAT_read(1, 1);
      count = (count_hi << 8) + count_low;
    }
  }
  
  if ((signal & SIGNAL_STOP)) // || count == 5)
  {
    EAT_write(1, 0, count & 0xFF);
    EAT_write(1, 1, (count >> 8) & 0xFF);
    return 255;
  }
  
  if ((signal & SIGNAL_TICK))
  {
    count++;
    Kernel_signal(SIGNAL_REDRAW);
    input_a = false;
    input_b = false;
  }

  if ((signal & SIGNAL_INPUT_A) && !input_a)
  {
    input_a = true;
    Kernel_signal(SIGNAL_REDRAW);
  }
  if ((signal & SIGNAL_INPUT_B) && !input_b)
  {
    input_b = true;
    Kernel_signal(SIGNAL_REDRAW);
  }
  
  return 0;
}

void setup()
{
  M5.begin();
  Serial.begin(115200);
  delay(30);
  Kernel_setup();
  int pid;
  pid = Kernel_start(&UI, 86400);
  Serial.printf("[UI   ] pid %d\n", pid);
  pid = Kernel_start(&PowerManagement, 1000);
  Serial.printf("[POWER] pid %d\n", pid);
  pid = Kernel_start(&UserInput, 0);
  Serial.printf("[INPUT] pid %d\n", pid);
  pid = Kernel_start(&test_func, 5000);
  Serial.printf("[TEST ] pid %d\n", pid);
  pid = Kernel_start(&Clock, 1000);
  Serial.printf("[CLOCK] pid %d\n", pid);
  pid = Kernel_start(&Battery, 2000);
  Serial.printf("[BATT ] pid %d\n", pid);

  M5.update();
  if (M5.BtnB.isPressed())
  {
    EAT_initialize();
  }
  else
  {
    EAT_load();
  }
}

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