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

#define IDLE_TIMEOUT 5000
#define SLEEP_TIMEOUT 10000

#define POWER_STATE_WAKE 0
#define POWER_STATE_IDLE 1
#define POWER_STATE_SLEEP 2

unsigned long accumulator = 0;
int power_state = 0;

void sleep()
{
  power_state = POWER_STATE_SLEEP;
  M5.Axp.DeepSleep();
}

void idle()
{
  power_state = POWER_STATE_IDLE;
  M5.Axp.ScreenBreath(7); 
}

void wake()
{
  power_state = POWER_STATE_WAKE;
  M5.Axp.ScreenBreath(12);
}

int PowerManagement(int pid, unsigned int signal)
{
  // Receive start/stop/input/tick signals
  if (signal & SIGNAL_START)
  {
    Kernel_signal_mask(pid, SIGNAL_START | SIGNAL_STOP | SIGNAL_INPUT_A | SIGNAL_INPUT_B | SIGNAL_TICK);
  }

  // Accumulate time every time we run
  if ((signal & SIGNAL_START) == 0 && signal & SIGNAL_TICK)
  {
    accumulator += Kernel_get_run_interval(pid);
  }

  // But reset it whenever a button is pressed
  if ((signal & SIGNAL_INPUT_A) != 0 || (signal & SIGNAL_INPUT_B) != 0)
  {
    if (power_state != POWER_STATE_WAKE)
    {
      wake();
    }
    accumulator = 0;
  }

  switch (power_state)
  {
    case POWER_STATE_WAKE:
      if (accumulator > IDLE_TIMEOUT)
      {
        idle();
      }
      break;
    case POWER_STATE_IDLE:
      if (accumulator > SLEEP_TIMEOUT)
      {
        sleep();
      }
      break;
  }

  // TODO: On sleep, signal "STOP" to all tasks and reschedule ourselves more frequently
  // Wait until Kernel_count_running == 1, then put chip to sleep

  return 0;
  
}