//Screen
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 display(128, 64, &Wire, -1);

//Wifi
#include <ESP8266WiFi.h>
#include <ESP8266mDNS.h>
#include <WiFiUdp.h>
#include <ArduinoOTA.h>

//Scale
#include "HX711.h"
HX711 scale;

//Memory
#include <Preferences.h>
Preferences Memory;

//Encoder
#include "Adafruit_seesaw.h"
Adafruit_seesaw ss;
#define SS_SWITCH 24
#define SEESAW_ADDR 0x36

/*To do
  -Adapt for new canning line.
  -Sensor for can IndexArm
  -Design new head that bolts on
  -Reduce I2C speed for longer screen cable or use booster
  -Remove as many global variables as possible

  Ints
    int8_t	-128 to 127
    uint8_t	0 to 255
    int16_t	-32768 to 32767
    uint16_t 0 to 65535
    int32_t	-2147483648 to 2147483647
    uint32_t 0 to 4294967295
	
  Overflow
    if((millis() - start) >= delay);  		// CORRECT version
    while ((millis() - start) <= delay);	// CORRECT version

  Text height width
    1 - 8     6
    2 - 16    12
    3 - 24    18
    4 - 32    24

	Preferences
    putChar		- bool
    putUChar
    putShort	- int8
    putUShort
    putInt		- int16
    putUInt
    putLong
    putULong
    putLong64
    putULong64
    putFloat
    putDouble
    putBool
    putString
    putBytes
*/

//Pinout

//D0 - OLED reset?
#define alarmPin  5   	  //D1 - Relay1 - Alarm
#define IndexArmPin 4		  //D2 - Relay2 - Index arm
#define IndexArmSensor 0 	//D3 - Index arm sensor - pulled up
//Empty			GPIO 02		    //D4 - Empty Pulled up
#define SCLI2C 14	    	  //D5 - SCL 
#define SDAI2C 12	    	  //D6 - SDA
#define HX711DT 13	   	  //D7 - HX711 DT
#define HX711SCK 15	  	  //D8 - HX711 SCK
  //RX GPIO 03
  //TX GPIO 01
  //A0

  /*
  HX711
    E+ Positive
    E- Ground
    A- White
    A+ Green 

  Can sensor
    brown - positive
    blue - negative
    black - signal
    high when no can
    low when can
    voltage divider 300k, 110k
  */

//Variables
  bool stable = false;      	//is the reading stable
  bool alarmHigh;
  bool alarmLow;
  bool canSize;     			//What can size is in use
  uint8_t curArrayValue = 0;	//Tracks where we are in the circular array
  uint8_t numReadings;  		//Number of readings for a stable value
  uint8_t stableDiff;   		//Stable reading difference value
  uint16_t indexDelay;			//Time between pushes
  uint16_t indexTime;			//Time the piston is extended
  uint16_t FGValue	;        	//The current beer FG
  float reading = 0;        	//The current reading
  float readingArray[128];  	//Array for graph values
  float steadyArray[5];	//Array for steady reading values
  float lowFill = 0;        	//Fill level volume allowance
  float highFill = 0;       	//Fill volume high level
  float fillVolume = 0;     	//Target fill volume

void setup() {
  //Set pins
    pinMode(alarmPin, OUTPUT);
    pinMode(IndexArmSensor, INPUT);
    pinMode(IndexArmPin, OUTPUT);
  
  //Start screen - remove the delay from here
    ScreenStartup();

  //Encoder
    if (!ss.begin(SEESAW_ADDR)) {
      display.clearDisplay();
      display.setTextSize(3);
      CenterJustify("Encoder", 0);
      display.setTextSize(1);
      CenterJustify("Unable to connect", 45);
      display.display();
      digitalWrite(alarmPin, HIGH);
      uint32_t timer = millis();
      while (ss.digitalRead(SS_SWITCH) && (millis() - timer) < 10000) {delay(10);} //Wait for tare button to be pressed or time out after 10 seconds
      digitalWrite(alarmPin, LOW);
    }
    ss.pinMode(SS_SWITCH, INPUT_PULLUP);
  
  //Load variables from memory
    Memory.begin("Settings", true);								//Opens memory file in read only
    //Bools
      bool initial = Memory.getBool("initial", true);             //Default run initialization
      alarmHigh = Memory.getBool("alarmHigh", false);   			//Default off
      alarmLow = Memory.getBool("alarmLow", true);   				//Default on
      canSize = Memory.getBool("canSize", true);                  //Default 473 mL
    //uint8_t
    numReadings = Memory.getUShort("NRead", 4);                 //Number of readings for a stable value
      stableDiff = Memory.getUShort("SDiff", 5);                  //Reading difference for a stable value
    //uint16_t
    indexDelay = Memory.getUInt("indexDelay", 1000);			//Default 1 second
    indexTime = Memory.getUInt("indexTime", 1000);				//Default 1 second
      FGValue = Memory.getUInt("FGVal", 1012);					//Last used FG for can weight calculations
    //uint32_t
      uint32_t scaleTare = Memory.getULong("sTare", 488356);      //Calculated value prior to installation
    //floats
      float scaleCalibration = Memory.getFloat("sCal", 951.031);  //Calculated value prior to installation
    Memory.end();

  
  //Start scale
    scale.begin(HX711DT, HX711SCK); 
    if (scale.wait_ready_timeout(1000)) { //If scale responds within 1 second of being called
      scale.set_scale(scaleCalibration); 	//Load saved weight calibration
      scale.set_offset(scaleTare); 		//Load saved tare 
      reading = scale.get_units();
      float oldReading = reading;
      uint16_t time = millis();

      while (abs(reading - oldReading) < 5 && (millis() - time) <= 1000) { //Checks if scale is stable for 1 second
        oldReading = reading;
        reading = scale.get_units();
      }

      if ((millis() - time) >= 1000) { 	//If scale was stable for one second
        if (abs(reading) > 20) { 			//If reading is not within X value of saved tare - confirm if scale is empty 
          display.clearDisplay();
          display.setTextSize(1);
          CenterJustify("Weight detected", 15);
          CenterJustify("Using estimate", 35);
          display.display();
          digitalWrite(alarmPin, HIGH);
          delay(500);
          digitalWrite(alarmPin, LOW);
          delay(2000);

        } else {scale.tare();}				//If scale is stable and no weight is detected
      
      }else {		//If weight is not stable, skip tare and use calculated values
        display.clearDisplay();
        display.setTextSize(1);
        CenterJustify("Scale values uneven", 15);
        CenterJustify("Using estimate", 35);
        display.display();
        digitalWrite(alarmPin, HIGH);
        delay(500);
        digitalWrite(alarmPin, LOW);
        delay(5000);}

    }else { //If scale does not respond within one second
      display.clearDisplay();
      display.setTextSize(3);
      CenterJustify("Scale", 0);
      display.setTextSize(1);
      CenterJustify("Unable to connect", 45);
      display.display();
      digitalWrite(alarmPin, HIGH);
      uint32_t timer = millis();
      while (ss.digitalRead(SS_SWITCH) && (millis() - timer) < 10000) {delay(10);} //Wait for tare button to be pressed or time out after 10 seconds
      digitalWrite(alarmPin, LOW);
    }
  
  //Initialization
    if(initial == true) {
      display.clearDisplay();
      display.setTextSize(1);
      CenterJustify("Initialization", 0);
      CenterJustify("Initilization file", 35);
      CenterJustify("not found. Press", 45);
      CenterJustify("button to setup", 55);
      display.display();

      delay(50); //Debounce
      while (!ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be released from previous press
      delay(50); //Debounce
      while (ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be pressed

      MenuFGChange();
      MenuCanSize();
      MenuIndexDelay();
      MenuIndexTime();
      MenuAlarmHigh();
      MenuAlarmLow();
      MenuFillLevel();
      MenuNumberReadings();
      MenuStableDifference();
      MenuCalibration();
      MenuTare();
    
      Memory.begin("Settings", false); //Open flash memory in write mode
        Memory.putBool("initial", false);
      Memory.end();
    
      display.clearDisplay();
      display.setTextSize(2);
      CenterJustify("Initialization", 15);
      CenterJustify("Complete", 40);
      display.display();
      delay(2000); //Screen delay
      
    }

  CalcFillVolume();
}

void loop() {

  if(!ss.digitalRead(SS_SWITCH)) {Tare();} //When tare button is pressed

  SensorReading(); //Takes a reading

  ScreenUpdate(); //Updates the screen with latest reading

  Alarm(); //Determines if the reading should trigger an alarm
  
  IndexArm(); //Determines if the index arm should be moved
  
}

void Tare() {
  
  //Displays 'Tare' on the screen when button is pressed
  digitalWrite(alarmPin, LOW);
  display.clearDisplay();
  display.setTextSize(3);
  CenterJustify("Tare", 15); 
  display.display();

  delay(50); //Debounce
  
  uint32_t time = millis();
  while(!ss.digitalRead(SS_SWITCH)){
    if ((millis() - time) >= 5000) { //If button is held for more than 5 seconds
      MainMenu();
      return; //Exit the loop without taring
    }
    delay(50);
  }

  scale.tare();
  display.setTextSize(1);
  CenterJustify(String(scale.get_offset()), 56); //Displays the tare value
  display.display();

  delay(50); //Debounce
}

void SensorReading() {
	
  static uint8_t steadyArrayNumber = 0; //Tracks where we are in the steady circular array
  reading = scale.get_units(); //Get current reading

  steadyArray[steadyArrayNumber++] = reading; //Add reading to array
  if(steadyArrayNumber >= numReadings){steadyArrayNumber = 0;} //Set the circular buffer index back to zero when it reaches the end

  switch(numReadings){
    case 2:
      if(abs(steadyArray[0] - steadyArray[1]) < (stableDiff*0.1)) {stable = true;}
      else{stable = false;}
      break;
    case 3:
      if((abs(steadyArray[0] - steadyArray[1]) < (stableDiff*0.1)) && (abs(steadyArray[1] - steadyArray[2]) < (stableDiff*0.1))) {stable = true;}
      else{stable = false;}
      break;
    case 4:
      if((abs(steadyArray[0] - steadyArray[1]) < (stableDiff*0.1)) && (abs(steadyArray[1] - steadyArray[2]) < (stableDiff*0.1)) && (abs(steadyArray[2] - steadyArray[3]) < (stableDiff*0.1))) {stable = true;}
      else{stable = false;}
      break;
    case 5:
      if((abs(steadyArray[0] - steadyArray[1]) < (stableDiff*0.1)) && (abs(steadyArray[1] - steadyArray[2]) < (stableDiff*0.1)) && (abs(steadyArray[2] - steadyArray[3]) < (stableDiff*0.1)) && (abs(steadyArray[3] - steadyArray[4]) < (stableDiff*0.1))) {stable = true;}
      else{stable = false;}
      break;
  }

  if (stable == true){
    readingArray[curArrayValue++] = reading; //Add reading to screen array
    if(curArrayValue >= 128){curArrayValue = 0;}} //Set the circular buffer index back to zero when it reaches the end
  else{
    readingArray[curArrayValue++] = 0; //Add 0 to screen array
    if(curArrayValue >= 128){curArrayValue = 0;} //Set the circular buffer index back to zero when it reaches the end
  }

}

void ScreenUpdate() {
  uint8_t xValue = curArrayValue;
  uint16_t graphUpper = highFill + 4; //upper end of graph
  uint16_t graphLower = lowFill - 5; //Lower end of graph

  display.clearDisplay();  
	for(uint8_t i = 0; i < display.width(); i++){ //Displays graph
		if (xValue >= 128){xValue = 0;} //Loops back when at upper limit
		int displayReading = readingArray[xValue];
		uint8_t lineHeight = constrain(map(displayReading, graphLower, graphUpper, 0, 47), 0, 47);
		uint8_t yPos = display.height() - lineHeight - 1; //Added a -1 to prevent screen rollover. May have to be removed
		display.drawFastVLine(i, yPos, lineHeight, SSD1306_WHITE);
		xValue++; 
	}
  display.drawFastHLine(0, display.height() - map(highFill, graphLower, graphUpper, 0, 47), 128, WHITE); //High fill line based on FG
  display.drawFastHLine(0, display.height() - map(fillVolume, graphLower, graphUpper, 0, 47), 128, WHITE); //Target fill line based on FG
  display.drawFastHLine(0, display.height() - map(lowFill, graphLower, graphUpper, 0, 47), 128, WHITE); //Low level fill line based on FG
  display.setTextSize(2);
  CenterJustify(String(reading, 1), 0);//Center justify the current reading
  display.setTextSize(1);
  display.setCursor(0, 0);
  display.print(FGValue * 0.001, 3); //Display FG on top left
  if (canSize) {TopRightJustify("473mL");} //Can size on top right
  else {TopRightJustify("355mL");} 
  LeftJustify(String(highFill, 1) + "g", 17); //Target fill weight  
  LeftJustify(String(fillVolume, 1) + "g", 37); //Target fill weight
  BottomLeftJustify(String(lowFill, 1) + "g"); //Low fill weight

  
  display.display();
}

void Alarm(){

	if (stable == true) { //If reading is stable for last X readings
	  switch(canSize){
      case 0: //355 mL
        if (50 < reading && reading < lowFill) {//If reading is between 50 and low fill value
          if (alarmLow == true) {digitalWrite(alarmPin, HIGH);}
        
        }else if (reading > highFill) {//If reading is above high fill volume
          if (alarmHigh == true) {digitalWrite(alarmPin, HIGH);}
        } 
        break;
      case 1: //473 mL
        if (50 < reading && reading < lowFill) {//If reading is between 50 and low fill value
          if (alarmLow == true) {digitalWrite(alarmPin, HIGH);}
        
        }else if (reading > highFill) {//If reading is above high fill volume
          if (alarmHigh == true) {digitalWrite(alarmPin, HIGH);}
        }
        break;
	  }
  
  }else {digitalWrite(alarmPin, LOW);} //If reading is not stable turn off alarm
} 

void IndexArm() {
	static bool pushing = false;
	static uint32_t lastIndexTime = 0;

	// If not currently pushing and can is detected, and enough delay has passed
	if (!pushing && (digitalRead(IndexArmSensor) == LOW) && (millis() - lastIndexTime >= indexDelay)) {
		digitalWrite(IndexArmPin, HIGH);
		lastIndexTime = millis();
		pushing = true;
		
	// If currently pushing and 1 second has passed, turn off the pusher
	}else if (pushing && (millis() - lastIndexTime >= indexTime)) {
		digitalWrite(IndexArmPin, LOW);
		pushing = false;
	}
}

void CalcFillVolume() {
  uint16_t size;
  if (canSize) {size = 473;}
  else {size = 355;}
  
  Memory.begin("Settings", true); // Opens memory file in read only
    uint8_t canWeight = Memory.getUShort("canWt", 15);
    uint8_t volAllowance = Memory.getUShort("vDiff", 2);
  Memory.end();

  fillVolume = ((FGValue * 0.001) * size) + canWeight; 
  highFill = (((FGValue * 0.001) * size) * (1 + (volAllowance * 0.01))) + canWeight;
  lowFill = (((FGValue * 0.001) * size) * (1 - (volAllowance * 0.01))) + canWeight;

}

void MainMenu() {
  uint8_t menu = 0; //0-Beer change, 1-Can Change, 2-Current settings, 3-Index delay, 4-Index time, 5-High Alarm, 6-Low alarm, 7-Fill level, 8-Tare, 9-Calibration, 10-Wifi
  int32_t delta = 0;

  MainMenuDisplay(menu);
  uint32_t menuTimer = millis();
  while (!ss.digitalRead(SS_SWITCH)) {//Waits for the button to be released to continue with program
    if(millis() > (menuTimer + 30000)) {MenuWifiUpdate();} //Backup method for wifi update
    delay(10);} //WDT reset
  delay(50); //Debounce

  delta = ss.getEncoderDelta(); //reset the encoder
  delta = 0; //Reset the delta before entering the loop

  menuTimer = millis();
  while (millis() < menuTimer + 10000) { //Main menu loop
    delta = ss.getEncoderDelta();
		if(delta != 0) {
      menu = constrain(menu + delta, 0, 13);
      MainMenuDisplay(menu); 
      menuTimer = millis(); //Reset the menu timer
    
    }else if (!ss.digitalRead(SS_SWITCH)) { //If button was held for more than 2 seconds
        switch(menu) {
          case 0: //Beer change - FG
            MenuFGChange();
            break;
          case 1: //Can change - Can size and can weight
            MenuCanSize();
            break;
          case 2: //Dislpay current settings
            MenuSettings();
            break;
          case 3: //Index delay
            MenuIndexDelay();
            break;
          case 4: //Index time
            MenuIndexTime();
            break;
          case 5: //High level alarm
            MenuAlarmHigh();
            break;
          case 6: //Low level alarm
            MenuAlarmLow();
            break;
          case 7: //Fill level
            MenuFillLevel();
            break;
          case 8: //Number of stable readings
            MenuNumberReadings();
            break;
          case 9: //stable diff
            MenuStableDifference();
            break;
          case 10: //Tare
            MenuTare();
            break;
          case 11: //Calibration
            MenuCalibration();
            break;
          case 12: //Clear Memory
            MenuClearMemory();
            break;
          case 13: //Wifi
            MenuWifiUpdate();
            break;
      }
      MainMenuDisplay(menu);    
      delay(50); //Debounce
      while (!ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be released with WDT reset
      delay(50); //Debounce
      menuTimer = millis(); //Reset the menu timer
    }
    delay(10); //WDT reset
  }
}

void MainMenuDisplay(uint8_t x){
  uint8_t menuNumber = 14;
  String menuItem[menuNumber] = {"Beer Change", "Can Change", "Current Settings", "Index Delay", "Index Time", "High Alarm", "Low Alarm", "Fill Level", "# Readings", "Stable Diff", "Default Tare", "Calibration", "Clear Memory", "Wifi Update"};
    
  display.clearDisplay();
  display.setTextSize(2);
  CenterJustify("Main Menu", 0);
  display.setTextSize(1);

  if(x == 0){
    display.fillRect(0, 35, display.width(), 8, SSD1306_WHITE);
    display.setTextColor(BLACK, WHITE); //Invert colour
    CenterJustify(menuItem[x], 35);
    display.setTextColor(WHITE, BLACK);
    CenterJustify(menuItem[x+1], 45);
    CenterJustify(menuItem[x+2], 55);
    
  }else if(x == menuNumber - 1) {
    CenterJustify(menuItem[x-2], 35);
    CenterJustify(menuItem[x-1], 45);
    display.fillRect(0, 55, display.width(), 8, SSD1306_WHITE);
    display.setTextColor(BLACK, WHITE); //Invert colour
    CenterJustify(menuItem[x], 55);
    display.setTextColor(WHITE, BLACK);
    
  }else{
    CenterJustify(menuItem[x-1], 35);
    display.fillRect(0, 45, display.width(), 8, SSD1306_WHITE);
    display.setTextColor(BLACK, WHITE); //Invert colour
    CenterJustify(menuItem[x], 45);
    display.setTextColor(WHITE, BLACK);
    CenterJustify(menuItem[x+1], 55);
  }
  display.display();
}

void SubMenuScrollingDisplay(uint16_t minVal, uint16_t currentVal, uint16_t maxVal, float multiplier, uint8_t decimal, String unit, String menuTitle) {
	
	display.clearDisplay();
  display.setTextSize(2);
  CenterJustify(menuTitle, 0);
  display.setTextSize(1); 
	
  if(currentVal == minVal){
    display.fillRect(0, 35, display.width(), 8, SSD1306_WHITE);
    display.setTextColor(BLACK, WHITE); //Invert colour
    CenterJustify(String(currentVal * multiplier, decimal) + unit, 35);
    display.setTextColor(WHITE, BLACK);
    CenterJustify(String((currentVal + 1) * multiplier, decimal) + unit, 45);
    CenterJustify(String((currentVal + 2) * multiplier, decimal) + unit, 55);
	  
	}else if(currentVal == maxVal) {
    CenterJustify(String((currentVal - 2) * multiplier, decimal) + unit, 35);
    CenterJustify(String((currentVal - 1) * multiplier, decimal) + unit, 45);
    display.fillRect(0, 55, display.width(), 8, SSD1306_WHITE);
    display.setTextColor(BLACK, WHITE); //Invert colour
    CenterJustify(String(currentVal * multiplier, decimal) + unit, 55);
    display.setTextColor(WHITE, BLACK);
	  
	}else{
    CenterJustify(String((currentVal - 1) * multiplier, decimal) + unit, 35);
    display.fillRect(0, 45, display.width(), 8, SSD1306_WHITE);
    display.setTextColor(BLACK, WHITE); //Invert colour
    CenterJustify(String(currentVal * multiplier, decimal) + unit, 45);
    display.setTextColor(WHITE, BLACK);
    CenterJustify(String((currentVal + 1) * multiplier, decimal) + unit, 55);
  }
	
  display.display();
	
}

void SubMenuBoolDisplay(bool currentVal, String menuTitle, String mainText, String option1, String option2) {
	
	display.clearDisplay();
	display.setTextSize(2);
	CenterJustify(menuTitle, 0);
	display.setTextSize(1);
	CenterJustify(mainText, 30);
	
	if (!currentVal) { 
    display.setTextColor(BLACK, WHITE); //Invert colour
    LeftJustify(option1, 50);
    display.setTextColor(WHITE, BLACK);
    RightJustify(option2, 50);}
  else{ 
    display.setTextColor(WHITE, BLACK);
    LeftJustify(option1, 50);
    display.setTextColor(BLACK, WHITE);
    RightJustify(option2, 50);
    display.setTextColor(WHITE, BLACK);
  }
  display.display();
	
}

uint16_t SubMenuScrolling(uint16_t minVal, uint16_t currentVal, uint16_t maxVal, float multiplier, uint8_t decimal, String unit, String menuTitle) {
	int32_t delta = 0;
	uint16_t originalVal = currentVal;

	SubMenuScrollingDisplay(minVal, currentVal, maxVal, multiplier, decimal, unit, menuTitle); //Update display
  
	while (!ss.digitalRead(SS_SWITCH)) {delay(10);} //Waits for button release with WDT reset
	delay(50); //Debounce
	delta = ss.getEncoderDelta(); //reset the encoder
	delta = 0; //Reset the delta before entering the loop
	
	uint32_t menuTimer = millis();
	while (millis() - menuTimer <= 10000) { //Exit loop after 10 seconds
		delta = ss.getEncoderDelta();
		if(delta != 0) {
			currentVal = constrain(currentVal + delta, minVal, maxVal); //Change value based on delta
			SubMenuScrollingDisplay(minVal, currentVal, maxVal, multiplier, decimal, unit, menuTitle); //Update display
			menuTimer = millis();
		
		}else if (!ss.digitalRead(SS_SWITCH)) {
			display.clearDisplay();
			display.setTextSize(2);
			CenterJustify(menuTitle, 0);
			display.setTextSize(1);
			CenterJustify("Value saved", 45);
			display.display();
			delay(2000); //Screen delay
			return(currentVal);
		}
    delay(20); //WDT Reset
	}
	return(originalVal);
}

bool SubMenuBool(bool currentVal, String menuTitle, String mainText, String option1, String option2) {
	
  int32_t delta = 0;
  bool originalVal = currentVal;

  SubMenuBoolDisplay(currentVal, menuTitle, mainText, option1, option2); //Update display
	
  while (!ss.digitalRead(SS_SWITCH)) {delay(10);} //Waits for button release
  delay(50); //Debounce
  delta = ss.getEncoderDelta(); //reset the encoder
  delta = 0; //Reset the delta before entering the loop
  
  uint32_t menuTimer = millis();
  while (millis() - menuTimer <= 10000) { //Exit loop after 10 seconds
    delta = ss.getEncoderDelta();
    if(delta != 0) {
      currentVal = !currentVal;
      SubMenuBoolDisplay(currentVal, menuTitle, mainText, option1, option2); //Update display
      menuTimer = millis();
      
    }else if (!ss.digitalRead(SS_SWITCH)) { //If button is pressed
      display.clearDisplay();
      display.setTextSize(2);
      CenterJustify(menuTitle, 0);
      display.setTextSize(1);
      CenterJustify("Value saved", 45);
      display.display();
      delay(2000); //Screen delay
      return(currentVal);
    }
    delay(20); //WDT reset
  }
  return(originalVal);
}

void MenuFGChange() {
	FGValue = SubMenuScrolling(900, FGValue, 1100, 0.001, 3, "", "Select FG");
	Memory.begin("Settings", false); //Open flash memory in write mode
		Memory.putUInt("FGVal", FGValue);
	Memory.end();
	CalcFillVolume();
}

void MenuCanSize() {
	canSize = SubMenuBool(canSize, "Can Size", "Which can size?", "   355ml   ", "   473ml   ");
	Memory.begin("Settings", false); // Opens memory file in write mode
		canSize = Memory.putBool("canSize", canSize);
		uint8_t canWeight = Memory.getUShort("canWt", 15);
		canWeight = SubMenuScrolling(5, canWeight, 30, 1, 0, " g", "Can Weight");
		Memory.putUShort("canWt", canWeight);
	Memory.end();
	CalcFillVolume();
}

void MenuIndexDelay(){
	indexDelay = SubMenuScrolling(0, indexDelay, 2000, 1, 0, " ms", "Index Delay");
	Memory.begin("Settings", false); //Open flash memory in write mode
		Memory.putUInt("indexDelay", indexDelay);
	Memory.end();
}

void MenuIndexTime() {
	indexTime = SubMenuScrolling(0, indexTime, 2000, 1, 0, " ms", "Index Time");
	Memory.begin("Settings", false); //Open flash memory in write mode
		Memory.putUInt("indexTime", indexTime);
	Memory.end();
}

void MenuAlarmHigh() {
	alarmHigh = SubMenuBool(alarmHigh, "High Alarm", "Use High alarm?", "    OFF    ", "    ON    ");
	Memory.begin("Settings", false); //Open flash memory in write mode
		Memory.putBool("alarmHigh", alarmHigh);
	Memory.end();
}

void MenuAlarmLow() {
	alarmLow = SubMenuBool(alarmLow, "Low Alarm", "Use low alarm?", "    OFF    ", "    ON    ");
	Memory.begin("Settings", false); //Open flash memory in write mode
		Memory.putBool("alarmLow", alarmLow);
	Memory.end();
}

void MenuFillLevel() {
	Memory.begin("Settings", false); // Opens memory file in write mode
		uint8_t vDiff = Memory.getUShort("vDiff", 2);
		vDiff = SubMenuScrolling(1, vDiff, 3, 1, 0, " %", "Fill Level");
		Memory.putUShort("vDiff", vDiff);
	Memory.end();
	CalcFillVolume();
}

void MenuNumberReadings() {
	numReadings = SubMenuScrolling(1, numReadings, 5, 1, 0, "", "# Readings");
	Memory.begin("Settings", false); //Open flash memory in write mode
		Memory.putUShort("NRead", numReadings);
	Memory.end();
}

void MenuStableDifference() {
	stableDiff = SubMenuScrolling(1, stableDiff, 100, 0.1, 1, " g", "Stable Diff");
	Memory.begin("Settings", false); //Open flash memory in write mode
		Memory.putUShort("SDiff", stableDiff);
	Memory.end();
}

void MenuSettings(){ 

  Memory.begin("Settings", true); // Opens memory file in read only
    uint32_t scaleTare = Memory.getULong("sTare", 488356);
    float scaleCalibration = Memory.getFloat("sCal", 951.031);
    uint8_t canWeight = Memory.getUShort("canWt", 15);
    uint8_t volAllowance = Memory.getUShort("vDiff", 2);
  Memory.end();

  display.clearDisplay();
  display.setTextSize(2);
  CenterJustify("Settings", 0); //Title
  display.setTextSize(1);
  LeftJustify("FG: ", 25); display.print((FGValue * 0.001), 3); //FG Value
  LeftJustify("Can Size: ", 35); 
    if (canSize) {display.print("473mL");} //Can Size
	else {display.print("355mL");}
  LeftJustify("Can Weight: ", 45); display.print(canWeight); display.print("g"); //Can Weight
  LeftJustify("Index Delay: ", 55); display.print(indexDelay); display.print("ms");
  display.display();

  delay(50); //Debounce
  while (!ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be released from previous press
  delay(50); //Debounce
  while (ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be pressed

  display.clearDisplay();
  display.setTextSize(2);
  CenterJustify("Settings", 0);
  display.setTextSize(1);
  LeftJustify("Index Time: ", 25); display.print(indexTime); display.print("ms");
  LeftJustify("High Alarm: ", 35);
    if(alarmHigh == true) {display.print("Enabled");}
    else {display.print("Disabled");}
  LeftJustify("Low Alarm: ", 45);
    if(alarmLow == true) {display.print("Enabled");}
    else {display.print("Disabled");}
  LeftJustify("Fill error: ", 55); display.print(volAllowance); display.print("%");
  display.display();

  delay(50); //Debounce
  while (!ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be released from previous press
  delay(50); //Debounce
  while (ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be pressed

  display.clearDisplay();
  display.setTextSize(2);
  CenterJustify("Settings", 0);
  display.setTextSize(1);
  LeftJustify("Num Readings: ", 25); display.print(numReadings);
  LeftJustify("Reading Diff: ", 35); display.print(stableDiff*0.1, 1); display.print("g");
  LeftJustify("Tare: ", 45); display.print(scaleTare);
  LeftJustify("Calibration:", 55); display.print(scaleCalibration, 5);
  display.display();

  delay(50); //Debounce
  while (!ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be released from previous press
  delay(50); //Debounce
  while (ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be pressed
  
}

void MenuTare(){

  display.clearDisplay();
  display.setTextSize(2);
  CenterJustify("Default Tare", 0);
  display.setTextSize(1);
  CenterJustify("Remove all items", 35);
  CenterJustify("from the scale and", 45);
  CenterJustify("press the button", 55);
  display.display();

  delay(50); //Debounce
  while (!ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be released
  delay(50); //Debounce
  while (ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be pressed
  scale.tare();

  Memory.begin("Settings", false); //Open flash memory in write mode
    Memory.putULong("sTare", scale.get_offset());
  Memory.end();

  display.clearDisplay();
  display.setTextSize(2);
  CenterJustify("Tare", 0);
  display.setTextSize(1);
  CenterJustify("Value saved", 45);
  display.display();
  delay(2000); //Screen delay

}

void MenuCalibration(){

  display.clearDisplay();
  display.setTextSize(2);
  CenterJustify("Calibration", 0);
  display.setTextSize(1);
  CenterJustify("Remove any weights", 35);
  CenterJustify("from the scale and", 45);
  CenterJustify("press the button", 55);
  display.display();

  delay(50); //Debounce
  while (!ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be released from previous press
  delay(50); //Debounce
  while (ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be pressed

  scale.set_scale(); //Remove previous setting
  scale.tare(); //Set new tare with no calibration

  display.clearDisplay();
  display.setTextSize(2);
  CenterJustify("Calibration", 0);
  display.setTextSize(1);
  CenterJustify("Add a measured weight", 35);
  CenterJustify("of 485g to the scale", 45);
  CenterJustify("and press the button", 55);
  display.display();

  delay(50); //Debounce
  while (!ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be released from previous press
  delay(50); //Debounce
  while (ss.digitalRead(SS_SWITCH)) {delay(10);} //Wait for tare button to be pressed

  float cal = scale.get_units(10) / 485;
  scale.set_scale(cal);

  Memory.begin("Settings", false); //Open flash memory in write mode
    Memory.putFloat("sCal", cal);
  Memory.end();

  display.clearDisplay();
  display.setTextSize(2);
  CenterJustify("Calibration", 0);
  display.setTextSize(1);
  CenterJustify("Value saved", 40);
  display.display();
  delay(2000); //Screen delay

}

void MenuClearMemory() {
  bool clearMem = SubMenuBool(0, "Clear Memory", "Are you sure?", "    NO    ", "    Yes    ");
	  
  if (clearMem) {
    Memory.begin("Settings", false); //Open flash memory in write mode
      Memory.clear();
    Memory.end();
    ESP.restart();
  }

}

void MenuWifiUpdate() { 
  
  display.clearDisplay();
  display.setTextSize(2);
  CenterJustify("Wifi", 0);
  display.setTextSize(1);
  CenterJustify("Connecting....", 40);
  display.display();

  WiFi.mode(WIFI_STA);
  WiFi.begin("iPhony", "P@55w0rd");  // Connect to WiFi - defaults to WiFi Station mode
  
  uint32_t time = millis();
  while (WiFi.waitForConnectResult() != WL_CONNECTED) { //Waits for wifi to connect
    if(millis() > time + 60000) { //If unable to connect to wifi after 30 seconds
      display.clearDisplay();
      display.setTextSize(2);
      CenterJustify("Wifi", 0);
      display.setTextSize(1);
      CenterJustify("No wifi available", 40);
      display.display();
      digitalWrite(alarmPin, HIGH);
      delay(500);
      digitalWrite(alarmPin, LOW);
      delay(5000);
      ESP.restart();
    }
    delay(10); //Watchdog timer reset
  }

  display.clearDisplay();
  display.setTextSize(2);
  CenterJustify("Wifi", 0);
  display.setTextSize(1);
  CenterJustify("Ready for update", 40);
  display.display();
  
  ArduinoOTA.begin();  // Starts OTA
  
  time = millis();
  while(millis() < (time + 300000)) {//loops for 5 minutes
    ArduinoOTA.handle(); // Handles a code update request
    delay(200);
  }  
  display.clearDisplay();
  display.setTextSize(2);
  CenterJustify("Wifi", 0);
  display.setTextSize(1);
  CenterJustify("Update failed", 40);
  display.display();
  digitalWrite(alarmPin, HIGH);
  delay(500);
  digitalWrite(alarmPin, LOW);
  delay(5000);
  ESP.restart();
}