#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(8); // 7-12
}

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

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);
    wake();
  }

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

  // 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)
      {
        if (Kernel_count_running_tasks() == 1)
        {
          sleep();
        }
        else
        {
          Kernel_set_run_interval(pid, 50);
          Kernel_signal(SIGNAL_STOP);
        }
      }
      break;
  }

  return 0;
  
}