#include <Tiny4kOLED.h>

int leftState = 0, TPleft = 2;
int rightState = 0, TPright = 2;
const int LED = 7;

const int STDBYl = 2;
const int CHRGl  = 3;
const int STDBYr = 2;
const int CHRGr  = 3;

//Srites for different Charge status, only for Tiny4kOLED library
const uint8_t PotionNC[] PROGMEM = {
0x00,0x00,0x00,0x00,0x00,0x00,0x80,0x40,0x00,0x40,0x00,0x40,0x00,0x40,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x00,0x40,0x00,0x20,0x0A,0x00,0x00,0x00,0x00,0x00,0x00,0x15,0x00,0x20,0x00,0x80,0x00,0x00,0x00,
0x50,0x04,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x02,0xA8,
0x00,0x01,0x04,0x00,0x10,0x00,0x20,0x00,0x40,0x00,0x40,0x00,0x40,0x00,0x40,0x00,0x20,0x00,0x08,0x00,0x02,0x00};

const uint8_t PotionFC[] PROGMEM = {
0x00,0x00,0x00,0x00,0x00,0x00,0xDE,0xFE,0xFE,0xFE,0xFE,0xFE,0xFE,0xFE,0xFE,0xDE,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x80,0x40,0xA0,0xA0,0xDF,0xE3,0xF7,0xF7,0xF7,0xF7,0xE3,0xDF,0xA0,0xA0,0x40,0x80,0x00,0x00,0x00,
0xF8,0x06,0xF9,0xFE,0xFF,0xC7,0xF9,0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,0xF9,0x06,0xF8,
0x00,0x03,0x04,0x0B,0x17,0x2F,0x2F,0x5F,0x5F,0x5F,0x5F,0x5F,0x5F,0x5F,0x5F,0x2F,0x2F,0x17,0x0B,0x04,0x03,0x00 };

const uint8_t Potion80[] PROGMEM = {
0x00,0x00,0x00,0x00,0x02,0x10,0xC0,0xC1,0xC0,0xC0,0xC0,0xC0,0xC0,0xC4,0xCA,0xC4,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x80,0x40,0xA0,0xA0,0xDF,0xC0,0xC0,0xC0,0xE0,0xE0,0xE0,0xDF,0xA0,0xA0,0x40,0x80,0x00,0x00,0x00,
0xF8,0x06,0xF9,0xFE,0xFF,0xC7,0xF9,0xFE,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFE,0xF9,0x06,0xF8,
0x00,0x03,0x04,0x0B,0x17,0x2F,0x2F,0x5F,0x5F,0x5F,0x5F,0x5F,0x5F,0x5F,0x5F,0x2F,0x2F,0x17,0x0B,0x04,0x03,0x00 };

const uint8_t Potion60[] PROGMEM = {
0x00,0x00,0x00,0x00,0x00,0x00,0xC4,0xCA,0xC4,0xC0,0xC0,0xC0,0xC0,0xC0,0xC4,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x80,0x40,0x20,0x20,0x1F,0x00,0x00,0x00,0x00,0x00,0x00,0x1F,0x20,0x20,0x40,0x80,0x00,0x00,0x00,
0xF8,0x06,0xF9,0xFE,0xFF,0xC7,0xF9,0xFE,0xFC,0xFC,0xF8,0xF0,0xF0,0xF0,0xF0,0xF8,0xF8,0xFC,0xFC,0xF9,0x06,0xF8,
0x00,0x03,0x04,0x0B,0x17,0x2F,0x2F,0x5F,0x5F,0x5F,0x5F,0x5F,0x5F,0x5F,0x5F,0x2F,0x2F,0x17,0x0B,0x04,0x03,0x00 };

const uint8_t Potion40[] PROGMEM = {
0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0xC4,0xC0,0xC0,0xC0,0xC0,0xC0,0xC8,0xC0,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x80,0x40,0x20,0x20,0x1F,0x00,0x00,0x00,0x00,0x00,0x00,0x1F,0x20,0x20,0x40,0x80,0x00,0x00,0x00,
0xF8,0x06,0x01,0x00,0x00,0x38,0x06,0x81,0x80,0xC0,0xC0,0xE0,0xE0,0xE0,0xF0,0xF0,0xF8,0xF8,0xF0,0xF1,0x06,0xF8,
0x00,0x03,0x04,0x0B,0x17,0x2F,0x2F,0x5F,0x5F,0x5F,0x5F,0x5F,0x5F,0x5F,0x5F,0x2F,0x2F,0x17,0x0B,0x04,0x03,0x00 };

const uint8_t Potion20[] PROGMEM = {
0x00,0x00,0x00,0x00,0x00,0x00,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,
0x00,0x00,0x00,0x80,0x40,0x20,0x20,0x1F,0x00,0x00,0x00,0x00,0x02,0x00,0x1F,0x20,0x20,0x40,0x80,0x00,0x00,0x00,
0xF8,0x06,0x01,0x00,0x00,0x38,0x06,0x01,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x06,0xF8,
0x00,0x03,0x04,0x0A,0x16,0x2E,0x2C,0x5C,0x58,0x58,0x5C,0x5C,0x5C,0x5C,0x5E,0x2E,0x2E,0x16,0x0A,0x04,0x03,0x00 };


void setup() {
  pinMode(LED,  OUTPUT);    //Status LED
  pinMode(STDBYr, INPUT_PULLUP);   //TP4056 pins
  pinMode(CHRGr,  INPUT_PULLUP);
  pinMode(STDBYl, INPUT_PULLUP);
  pinMode(CHRGl,  INPUT_PULLUP);
  
  oled.begin(72, 40, sizeof(tiny4koled_init_72x40), tiny4koled_init_72x40);
  oled.setRotation(1);
  oled.setFont(FONT8X16);
  oled.clear();
  oled.on();

  Blink(LED);
  Blink(LED);
}

void loop() {
  //oled.clear();
  readPins();

switch(TPleft) {          //Set values for the PotionCharge Function dependent on readPins
    case   0: leftState = 100;
      break;
    case  1: leftState += 20; if (leftState > 80) leftState= 20;  
      break;
    case  2: leftState = 0;
      break;
  }
switch(TPright) {         //TPleft and TPright state becomes leftState and rightState
    case   0: rightState = 100;
      break;
    case  1: rightState += 20; if (rightState > 80) rightState= 20;  
      break;
    case  2: rightState = 0;
      break;
  }
  
  oled.setCursor(0,1);         //only for DEBUG
  oled.print(analogRead(CHRGl)); 
  oled.print(" ");oled.println(digitalRead(CHRGl));
  oled.print(analogRead(STDBYl));
  oled.print(" ");oled.println(digitalRead(STDBYl));
  //oled.print(TPleft);

  /*oled.setCursor(5,3);         //only for DEBUG
  oled.print(digitalRead(CHRGr));
  oled.print(" ");
  oled.print(digitalRead(STDBYr));
  oled.print(" - ");
  oled.print(TPright);
*/

  //PotionStage(1,leftState);     //Display the Potion state
  //PotionStage(2,rightState);

  oled.on();                    //update OLED
  delay(5);
}

void readPins() {                 //read TP4056 pin and output wich state: Charge = 1, Full = 0 or No Battery = 2
  if      (digitalRead(CHRGl) == 1 && digitalRead(STDBYl) == 1) TPleft = 2;
  else if (digitalRead(CHRGl) == 1 && digitalRead(STDBYl) == 0) TPleft = 1;
  else if (digitalRead(CHRGl) == 0 && digitalRead(STDBYl) == 1) TPleft = 0;

  if      (digitalRead(CHRGr) == 1 && digitalRead(STDBYr) == 1) TPright = 2;
  else if (digitalRead(CHRGr) == 1 && digitalRead(STDBYr) == 0) TPright = 1;
  else if (digitalRead(CHRGr) == 0 && digitalRead(STDBYr) == 1) TPright = 0;
}

void PotionStage(int PotionNr, int state) {
  int pos = map(PotionNr, 1, 2, 5, 45);      //set x-value(pos) of the screen for left  or right potion
  oled.setCursor(pos, 0);
  switch(state) {                            //display the respective Charge state Bitmap 
    case   0: oled.bitmap(pos,0,pos + 22,4,PotionNC);
      break;
    case  20: oled.bitmap(pos,0,pos + 22,4,Potion20);
      break;
    case  40: oled.bitmap(pos,0,pos + 22,4,Potion40);
      break;
    case  60: oled.bitmap(pos,0,pos + 22,4,Potion60);
      break;
    case  80: oled.bitmap(pos,0,pos + 22,4,Potion80);
      break;
    case 100: oled.bitmap(pos,0,pos + 22,4,PotionFC);
      break;
  }
}

void Blink(int pin) {               //Blink LED on pin
  digitalWrite(pin, HIGH);  
  delay(10);
  digitalWrite(pin, LOW);
}