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

int voltage_to_percent(float voltage);
void draw(int bars);

#define BATTERY_OFFSET_X        141
#define BATTERY_OFFSET_Y          1
#define BATTERY_WIDTH            17
#define BATTERY_HEIGHT            8
#define BATTERY_BARS              4
#define BATTERY_TERMINAL_HEIGHT   4
#define BATTERY_TERMINAL_WIDTH    2
#define BATTERY_COLOUR            0x999999
#define BATTERY_LOW_COLOUR        0xAA0000
#define BATTERY_PADDING           1
#define BATTERY_BAR_COLOUR        0xAAAAAA

int battery_bars = -1;

int Battery(int pid, unsigned int signal)
{
  if (signal & SIGNAL_START)
  {
    Kernel_signal_mask(pid, SIGNAL_START | SIGNAL_STOP | SIGNAL_TICK | SIGNAL_REDRAW);
  }
  if (signal & SIGNAL_STOP)
  {
    return 255;
  }
  if (signal & SIGNAL_TICK)
  {
    float vbat = M5.Axp.GetVbatData() * 1.1 / 1000;
    int percent = voltage_to_percent(vbat);
    int bars = floor((percent / (100.0 / BATTERY_BARS)) + 0.5);
    if (battery_bars != bars)
    {
      battery_bars = bars;
      Kernel_signal(SIGNAL_REDRAW);
    }
  }

  if (signal & SIGNAL_REDRAW)
  {
    draw(battery_bars);
  }

  return 0;
}

void draw(int bars)
{
  // Draw terminal
  M5.Lcd.fillRect(
     BATTERY_OFFSET_X,
     BATTERY_OFFSET_Y + (BATTERY_HEIGHT / 2) - (BATTERY_TERMINAL_HEIGHT / 2),
     BATTERY_TERMINAL_WIDTH,
     BATTERY_TERMINAL_HEIGHT,
     bars == 0 ? rgb_to_colour(BATTERY_LOW_COLOUR) : rgb_to_colour(BATTERY_COLOUR)
     );
  // Draw battery shell
  M5.Lcd.drawRect(
      BATTERY_OFFSET_X + BATTERY_TERMINAL_WIDTH,
      BATTERY_OFFSET_Y,
      BATTERY_WIDTH - BATTERY_TERMINAL_WIDTH,
      BATTERY_HEIGHT,
      bars == 0 ? rgb_to_colour(BATTERY_LOW_COLOUR) : rgb_to_colour(BATTERY_COLOUR)
      );

  // -2: for frame
  int bar_width = (BATTERY_WIDTH - BATTERY_TERMINAL_WIDTH - 2 - (BATTERY_PADDING * (BATTERY_BARS + 1))) / BATTERY_BARS;
  int bar_height = BATTERY_HEIGHT - (BATTERY_PADDING * 2) - 2;
  int x = BATTERY_OFFSET_X + BATTERY_WIDTH - 1 - BATTERY_PADDING - bar_width;
  int y = BATTERY_OFFSET_Y + 1 + BATTERY_PADDING;

  while (bars > 0)
  {
    
    M5.Lcd.fillRect(
      x,
      y,
      bar_width,
      bar_height,
      rgb_to_colour(BATTERY_BAR_COLOUR)
      );
    x -= (bar_width + BATTERY_PADDING);
    bars--;
  }
     
  //M5.Lcd.setCursor(80, 20);
  //M5.Lcd.print(bars);
}

int voltage_to_percent(float voltage)
{
  /*
    4.2 volts 100%
    4.1 about 90%
    4.0 about 80%
    3.9 about 60%
    3.8 about 40%
    3.7 about 20%
    3.6 empty for practical purposes
   */
  return ((voltage - 3.6) / (4.2-3.6)) * 100;
}