#include <LiquidCrystal.h>
LiquidCrystal lcd(7, 6, 2, 3, 4, 5);




//Input & Button Logic
const int numOfInputs = 4;
const int inputPins[numOfInputs] = {16,18,19,11};
int inputState[numOfInputs];
int lastInputState[numOfInputs] = {LOW,LOW,LOW,LOW};
bool inputFlags[numOfInputs] = {LOW,LOW,LOW,LOW};
long lastDebounceTime[numOfInputs] = {0,0,0,0};
long debounceDelay = 5;
int v_batt = 7;				// Voltage line from the battery
int volt_1 = 0;
int volt_2 = 1;

// Function declarations -----------------------------------------------------------------
void setInputFlags (void);
void resolveInputFlags (void);
void printScreen (void);
void parameterChange (int key);
void inputAction (int input);
float volt_pin (int pin);           // Read and display voltage
//-----------------------------------------------------------------------------------------

// Variables for voltage divider
float denominator;
int resistor1 = 22000; // Values measured in circuit
int resistor2 = 2200;  // for better accuracy
// ----------------------------------------------------

//LCD Menu Logic
const int numOfScreens = 5;
int currentScreen = 0;
String screens[numOfScreens][2] = {{"Voltage 1","Volts"}, {"Voltage 2","Volts"},
	{"Load current","Amps"},{"Batt Voltage","volts"}, {"SD Logging", ""}};
float parameters[numOfScreens];
String parameters_sd[2] = {"No","Yes"};

void setup() {
	for(int i = 0; i < numOfInputs; i++) {
		pinMode(inputPins[i], INPUT);
//		digitalWrite(inputPins[i], HIGH); // pull-up 20k
	}
	//Serial.begin(9600);
	lcd.begin(16, 2);

	denominator = (float)resistor2 / (resistor1 + resistor2);
}


void loop() {
	setInputFlags();
	resolveInputFlags();
	parameters[3] = volt_pin(v_batt);
	parameters[0] = volt_pin(volt_1);
	parameters[1] = volt_pin(volt_2);
//	printScreen();
//	delay(1000);
}

void setInputFlags() {
	for(int i = 0; i < numOfInputs; i++) {
		int reading = digitalRead(inputPins[i]);
	if (reading != lastInputState[i]) {
		lastDebounceTime[i] = millis();
	}
	if ((millis() - lastDebounceTime[i]) > debounceDelay) {
		if (reading != inputState[i]) {
			inputState[i] = reading;
		if (inputState[i] == HIGH) {
			inputFlags[i] = HIGH;
			}
		}
	}
		lastInputState[i] = reading;
	}
}

void resolveInputFlags() {
	for(int i = 0; i < numOfInputs; i++) {
		if(inputFlags[i] == HIGH) {
			inputAction(i);
			inputFlags[i] = LOW;
			printScreen();
		}
	}
}

void inputAction(int input) {
	if(input == 0) {
		if (currentScreen == 0) {
			currentScreen = numOfScreens-1;
		}else{
			currentScreen--;
			}
		}else if(input == 1) {
			if (currentScreen == numOfScreens-1) {
				currentScreen = 0;
		}else{
			currentScreen++;
			}
		}else if(input == 2) {
			parameterChange(0);
		}else if(input == 3) {
			parameterChange(1);
		}
}

void parameterChange(int key) {
	if(key == 0) {
		parameters[currentScreen]++;
	}else if(key == 1) {
		parameters[currentScreen]--;
	}
}

void printScreen() {
	lcd.clear();
	lcd.print(screens[currentScreen][0]);
	lcd.setCursor(0,1);
	lcd.print(parameters[currentScreen]);
	lcd.print(" ");
	lcd.print(screens[currentScreen][1]);
}


float volt_pin(int pin) {

	float voltage;
	//Obtain RAW voltage data
	voltage = analogRead(pin);

	//Convert to actual voltage
	voltage = (voltage / 1024) * 3.3;

	//Convert to voltage after divider
	voltage = voltage / denominator;

	return voltage;
}