#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.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);

int batteryPct = 0;
double battVoltage = 0.0;

static const double chargeModifier = -0.3;
static const double dischargeModifier = 0.3;

double voltageModifier = 0;

bool powerState = false;
bool screenOn = false;
bool charging_1 = false;
bool charging_2 = false;
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 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);
	display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
	display.clearDisplay();
	display.setTextColor(WHITE);
	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);
	battVoltage = GetVoltage();
	initScreen();
	delay(200);
}

void loop() {
	delay(100);
	checkPresses();
	checkCharging();
	if (screenState != 0) {updateScreen();}
}

void getBatteryCharge(){
	double voltage = GetVoltage();

	if (voltage > 4.13){batteryPct = 100;}
	else if (voltage > 4.06){batteryPct = 90;}
	else if (voltage > 3.99){batteryPct = 80;}
	else if (voltage > 3.92){batteryPct = 70;}
	else if (voltage > 3.85){batteryPct = 60;}
	else if (voltage > 3.78){batteryPct = 50;}
	else if (voltage > 3.71){batteryPct = 40;}
	else if (voltage > 3.64){batteryPct = 30;}
	else if (voltage > 3.57){batteryPct = 20;}
	else if (voltage > 3.50){batteryPct = 10;}
	else if (voltage > 3.40){
		Serial.println("We are fucked");
		batteryPct = 0;
	}
	else if (voltage < 2){batteryPct = -1;}

}


void checkPresses(){
	if (digitalRead(PWRBTN) == 0) {
		delay(100);
		if (digitalRead(PWRBTN) == 0) {
			blinkLed(PWRLED);
			if(powerState == true){
				turnOff();
			}
			else {
				turnOn();
			}
		}
	}
	if (digitalRead(CHGBTN) == 0) {
		delay(100);
		if (digitalRead(CHGBTN) == 0) {
			blinkLed(CHGLED);
			if (screenState == 1 || screenState == 0) {
				screenState++;
			}
			else {
				initScreen();
				screenState = 0;
			}
		}
	}
}

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() {
	digitalWrite(FETCTRL, HIGH);
	powerState = true;
}

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


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

void updateScreen() {
	getBatteryCharge();
	if (screenState == 1) {
		updateDisplayPct();
	}
	else if (screenState == 2) {
		updateDisplayVoltage();
	}
	updateDispayChg();
}

void updateDisplayPct(){
	display.clearDisplay();
	display.drawBitmap(0, 0, battery_logo, 120, 32, 1);
	display.setTextSize(2);
	display.setCursor(35, 10);
	display.setTextColor(0);
	display.print(batteryPct);
	display.println("%");
	display.display();
	screenOn = true;
}

void updateDisplayVoltage(){
	display.clearDisplay();
	display.drawBitmap(0, 0, battery_logo, 120, 32, 1);
	display.setTextSize(2);
	display.setCursor(30, 10);
	display.setTextColor(0);
	display.print(GetVoltage());
	display.println(" V");
	display.display();
	screenOn = true;
}

void updateDispayChg(){
	if (charging_1 == true) {display.drawBitmap(118, 0, charge_logo, 16, 14, 1);}
	if (charging_2 == true) {display.drawBitmap(118, 16, charge_logo, 16, 14, 1);}
	display.display();
}

double GetVoltage(){
	int totalSamples = 0;
	double voltage = 0;
	for (int q = 0; q < 10; q++) {
		 totalSamples = totalSamples + analogRead(VBAT);
		 delay(5);
		}
	voltage = (totalSamples / 10) * (5.00 / 1023.0);

	if(charging_1 == true || charging_2 == true){voltageModifier = chargeModifier;}
	else if(powerState == true){voltageModifier = dischargeModifier;}
	else {voltageModifier = 0;}

	voltage = voltage + voltageModifier;

	if (voltage > (battVoltage-0.03) && voltage < (battVoltage+0.03)) {
		return battVoltage;
	}
	else {
		battVoltage = voltage;
		return voltage;
	}
}

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