#include <MAX17043.h>

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

#include "Arduino.h"

#include "LowPower.h"
#include <avr/wdt.h>

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 32
#define OLED_RESET -1

#define PWRBTN 3
#define CHGBTN 4
#define PWRLED 5
#define CHGLED 6
#define VBAT A0
#define FETCTRL 2
#define CSTAT1 A1
#define CSTAT2 A2

#define DIMLEN 274

Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

bool powerState = false;
bool screenOn = false;
bool charging_1 = false;
bool charging_2 = false;
bool isSafe = true;
int screenState = 0;

static const unsigned char PROGMEM battery_logo[] = {
	B00000011, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11000000, B00000000,
	B00001111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11110000, B00000000,
	B00011111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111000, B00000000,
	B00111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111100, B00000000,
	B01111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111110, B00000000,
	B01111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111110, B00000000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B00000000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B00000000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B00000000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B00000000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B00000000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B01100000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B01110000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B01111000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B01111000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B01111000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B01111000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B01111000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B01111000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B01110000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B01100000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B00000000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B00000000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B00000000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B00000000,
	B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B00000000,
	B01111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111110, B00000000,
	B01111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111110, B00000000,
	B00111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111100, B00000000,
	B00011111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111000, B00000000,
	B00001111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11110000, B00000000,
	B00000011, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11111111, B11000000, B00000000
};

static const unsigned char PROGMEM charge_logo[] = {
	B00011111, B11000000,
	B00010000, B01000000,
	B00100000, B01000000,
	B00100000, B10000000,
	B01000001, B00000000,
	B01000011, B11000000,
	B10000000, B01000000,
	B11110000, B10000000,
	B00010001, B00000000,
	B00100010, B00000000,
	B00100100, B00000000,
	B01001000, B00000000,
	B01010000, B00000000,
	B01100000, B00000000
};

static const unsigned char PROGMEM charged_logo[] = {
	B00011111, B11000000,
	B00011111, B11000000,
	B00111111, B11000000,
	B00111111, B10000000,
	B01111111, B00000000,
	B01111111, B11000000,
	B11111111, B11000000,
	B11111111, B10000000,
	B00011111, B00000000,
	B00111110, B00000000,
	B00111100, B00000000,
	B01111000, B00000000,
	B01110000, B00000000,
	B01100000, B00000000
};

static const int dimmerVals[] = {
	0,0,0,0,0,0,0,0,0,0,0,0,1,1,1,1,1,2,2,2,2,3,3,4,4,4,5,5,6,6,7,7,8,8,9,9,10,11,11,12,13,14,15,16,17,18,19,20,21,
	22,24,25,26,28,29,30,32,33,35,37,38,40,42,44,46,48,49,52,54,56,58,60,63,65,67,70,73,75,78,80,83,86,89,92,95,98,
	101,105,108,112,116,120,124,127,131,136,139,144,148,152,156,160,164,168,172,177,181,185,189,192,196,200,204,208,
	211,215,218,221,224,227,230,233,236,238,240,242,244,246,248,249,250,251,252,253,253,253,253,253,253,253,253,253,
	253,252,252,251,249,248,247,245,243,241,239,236,234,231,228,225,222,219,216,212,208,205,201,197,194,190,186,182,
	178,174,169,165,161,157,153,149,145,141,137,133,129,125,121,117,113,110,106,102,98,96,93,89,87,84,81,78,76,73,71,
	68,65,63,61,58,56,54,52,50,48,46,44,42,40,39,37,35,34,32,31,29,28,26,25,24,23,21,20,19,18,17,16,15,14,13,12,12,11,
	10,9,9,8,8,7,7,6,6,5,5,4,4,4,3,3,2,2,2,1,1,1,1,1,1,0,0,0,0,0,0,0,0,0,0
};

void setup() {
	delay(100);
	wdt_enable(WDTO_8S);
	display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
	display.clearDisplay();
	display.setTextColor(WHITE);
	Wire.begin(); 
	Serial.begin(9600);
	pinMode(VBAT, INPUT);
	pinMode(CSTAT1, INPUT);
	pinMode(CSTAT2, INPUT);
	pinMode(FETCTRL, OUTPUT);
	pinMode(PWRLED, OUTPUT);
	pinMode(CHGLED, OUTPUT);
	pinMode(PWRBTN, INPUT_PULLUP);
	pinMode(CHGBTN, INPUT_PULLUP);
	digitalWrite(FETCTRL, LOW);
	resetScreen();
	delay(200);
	FuelGauge.begin();
	FuelGauge.reset(); //<== Might cause problems
	FuelGauge.quickstart();
}

void loop() {
	delay(100);
	wdt_reset();
	checkPresses();
	checkCharging();
	safetyCheck();
	if (screenState != 0 && isSafe == true) {updateScreen();}
	if ((charging_1 == true || charging_2 == true) && screenState == 0) {updateDisplayChg();}
	else{
		if (screenOn == true && screenState == 0){
			resetScreen();
		}
	}
	if ((powerState == false && screenState == 0) && (charging_1 == false && charging_2 == false)) {
		LowPower.powerDown(SLEEP_1S, ADC_OFF, BOD_OFF);
	}
}

void checkPresses(){
	if (digitalRead(PWRBTN) == 0) {
		if (powerState == true || screenState != 0) {
			delay(700);
		}
		if (digitalRead(PWRBTN) == 0) {
			if(powerState == true){
				turnOff();
			}
			else {
				turnOn();
			}
		}
	}
	if (digitalRead(CHGBTN) == 0) {
		if (powerState == true || screenState != 0) {
			delay(700);
		}
		if (digitalRead(CHGBTN) == 0) {
			if(isSafe == true){
				blinkLed(CHGLED);
				if (screenState == 1 || screenState == 0) {
					screenState++;
				}
				else {
					resetScreen();
					screenState = 0;
				}
			}
			else {
				errorBlink();
			}
		}
	}
}

void checkCharging(){
	if (digitalRead(CSTAT1) == 1){
		delay(100);
		if (digitalRead(CSTAT1) == 1){
		charging_1 = true;
		}
	}
	else { charging_1 = false;
	}

	if (digitalRead(CSTAT2) == 1){
		delay(100);
		if (digitalRead(CSTAT2) == 1){
		charging_2 = true;
		}
	}
	else { charging_2 = false;
	}
}

void turnOn() {
	if(isSafe == true){
		blinkLed(PWRLED);
		digitalWrite(FETCTRL, HIGH);
		powerState = true;
	}
	else{
		errorBlink();
	}
}

void turnOff() {
	blinkLed(PWRLED);
	digitalWrite(FETCTRL, LOW);
	powerState = false;
}

void resetScreen() {
	display.clearDisplay();
	display.display();
	screenOn = false;
}

void updateScreen(){
	display.clearDisplay();
	display.drawBitmap(0, 0, battery_logo, 120, 32, 1);
	display.setTextSize(2);
	display.setCursor(30, 10);
	display.setTextColor(0);
	if (screenState == 1){
		display.print(getPct());
		display.println("%");
	}
	else  if (screenState == 2){
		display.print(getVoltage());
		display.println(" V");
	}
	updateDispayHelper();
	display.display();
	screenOn = true;
}

void updateDisplayChg(){
	display.clearDisplay();
	updateDispayHelper();
	display.display();
	screenOn = true;
}

void updateDispayHelper(){
	int batteryPct = getPct();
	if (charging_1 == true && batteryPct != 100) {display.drawBitmap(118, 0, charge_logo, 16, 14, 1); screenOn = true;}
	else if (charging_2 == true && batteryPct != 100) {display.drawBitmap(118, 16, charge_logo, 16, 14, 1); screenOn = true;}
	else if (charging_1 == true && batteryPct == 100) {display.drawBitmap(118, 0, charged_logo, 16, 14, 1); screenOn = true;}
	else if (charging_2 == true && batteryPct == 100) {display.drawBitmap(118, 16, charged_logo, 16, 14, 1); screenOn = true;}
}

double getVoltage(){
	return (FuelGauge.voltage()/1000);
}

int getPct(){
	return (FuelGauge.percent());
}

void safetyCheck(){
	double currentVoltage = getVoltage();
	if(currentVoltage > 3.40 ){ //|| currentVoltage < 1.00
		isSafe = true;
	}
	else{
		isSafe = false;
		if(powerState == true){
			turnOff();
			screenState = 0;
			resetScreen();
		}
	}
}

void blinkLed(int ctrlLed) {
	for(int i=0; i<DIMLEN; i++){
		analogWrite(ctrlLed, dimmerVals[i]); delay(12);
	}
}

void errorBlink() {
	for(int i=0; i<5; i++){
		digitalWrite(PWRLED, 1);
		digitalWrite(CHGLED, 1);
		delay(400);
		digitalWrite(PWRLED, 0);
		digitalWrite(CHGLED, 0);
		delay(400);
	}
}