/* EZ-ViZi Level - 1.0 24/10/2022
   COLEO Tools (bv?)
   For Nano, maybe
*/

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_NeoPixel.h>
#include <Adafruit_SSD1306.h>
#include <Button.h>
#include <EEPROMex.h>

Adafruit_MPU6050 mpu;

String softwareVersion = "v1.0 Beta  24/10/2022";
const float alpha = 0.1;
double Xg,Yg,Zg,avXg,avYg,avZg;
double fXg = 0;
double fYg = 0;
double fZg = 0;
int avg=20; // number of times to average over
double pitchd, rolld, disp_roll, disp_pitch;
int rollones, rolltens, rollhundreds, rolltenths, rollxten;
int led_index = 0; // active led index
double roll_corr = -0.19; //hard-coded roll measurement correction (due to sensor mounting orientation inside case)
double pitch_corr = -1.27;//hard-coded pitch measurement correction
double range = 45; // plus/minus angle to display in degrees
float settingsStartTime = 0;      //To use with menuTimeout to track if we should exit
const int settingsTimeout = 3000; // 5 seconds

Button upBtn(27); // Pin 14
Button downBtn(14); // Pin 27
Button unitsBtn(12); // Pin 12

int delayval = 00; // timing delay in milliseconds

//Settings
bool percentOn;
int addressPercentOn;
int limitAngle;
int addressLimitAngle;
float rollCorr;
int addressRollCorr;
float pitchCorr;
int addressPitchCorr;

#define LEDPin 2 // LED
#define N_LEDS 17

#define OLED_ADDR   0x3C
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
// Declaration for an SSD1306 display connected to I2C (SDA, SCL pins)
#define OLED_RESET -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

Adafruit_NeoPixel pixels(N_LEDS, LEDPin, NEO_GRB + NEO_KHZ800);
uint32_t r = pixels.Color(255, 0, 0);
uint32_t g = pixels.Color(0, 255, 0);
uint32_t b = pixels.Color(0, 0, 255);
uint32_t y = pixels.Color(180, 40, 0);
uint32_t w = pixels.Color(80, 80, 80);
uint32_t off = pixels.Color(0, 0, 0);
uint32_t barcolor;

static const unsigned char logo_bmp[] PROGMEM = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x01, 0x0c, 0x30, 0x07, 0x38, 0x00, 0xff, 0xe0, 0x0b, 0xfc, 0xfe, 0x00, 0x63, 0x00, 0x00, 
  0x00, 0x03, 0x06, 0xf0, 0x1f, 0x1e, 0x01, 0xff, 0xe0, 0x0f, 0xfc, 0xfe, 0x01, 0xe0, 0xe0, 0x00, 
  0x00, 0x0f, 0x03, 0xf0, 0x3e, 0x0f, 0x80, 0x7f, 0xc0, 0x07, 0xfc, 0x7e, 0x03, 0xc1, 0xf0, 0x00, 
  0x00, 0x1f, 0x01, 0x70, 0x7e, 0x0f, 0xc0, 0x7f, 0x80, 0x03, 0xfc, 0x1e, 0x07, 0xc0, 0xf8, 0x00, 
  0x00, 0x3f, 0x00, 0x70, 0xfe, 0x0f, 0xe0, 0x7f, 0x80, 0x03, 0xfc, 0x0c, 0x0f, 0xc0, 0xf0, 0x00, 
  0x00, 0x3f, 0x00, 0x70, 0xfe, 0x0f, 0xe0, 0x7f, 0x80, 0x03, 0xfc, 0x06, 0x1f, 0xc0, 0xfe, 0x00, 
  0x00, 0x7f, 0x00, 0x71, 0xfe, 0x0f, 0xf0, 0x7f, 0x80, 0x03, 0xfc, 0x06, 0x1f, 0xc0, 0xfe, 0x00, 
  0x00, 0x7f, 0x00, 0x71, 0xfc, 0x0f, 0xf0, 0x7f, 0x80, 0x03, 0xfc, 0x00, 0x3f, 0xc0, 0xfa, 0x00, 
  0x00, 0x7f, 0x00, 0x31, 0xfc, 0x0f, 0xf0, 0x7f, 0x80, 0x03, 0xfc, 0x30, 0x3f, 0xc0, 0xff, 0x00, 
  0x00, 0x6f, 0x00, 0x23, 0xfc, 0x0f, 0xf0, 0x7f, 0x80, 0x03, 0xfc, 0x30, 0x3f, 0xc0, 0xff, 0x00, 
  0x00, 0xe7, 0x00, 0x03, 0xfe, 0x0f, 0xf8, 0x7f, 0x80, 0x03, 0xfc, 0x70, 0x3f, 0xc0, 0xff, 0x00, 
  0x00, 0xef, 0x00, 0x03, 0xfe, 0x0f, 0xf8, 0x7f, 0x80, 0x03, 0xfc, 0xf0, 0x3f, 0xc0, 0xff, 0x00, 
  0x00, 0xff, 0x00, 0x03, 0xfc, 0x0f, 0xf8, 0x7f, 0x80, 0x03, 0xfd, 0xf0, 0x3f, 0xc0, 0xff, 0x00, 
  0x00, 0xff, 0x00, 0x03, 0xfe, 0x0f, 0xf8, 0x7f, 0x80, 0x03, 0xfc, 0xf0, 0x3f, 0xc0, 0xff, 0x00, 
  0x00, 0xff, 0x00, 0x03, 0xfe, 0x0f, 0xf8, 0x7f, 0x80, 0x03, 0xfc, 0x70, 0x3f, 0xc0, 0xff, 0x00, 
  0x00, 0xff, 0x00, 0x03, 0xfe, 0x0f, 0xf8, 0x7f, 0x80, 0x03, 0xfc, 0x30, 0x3f, 0xc0, 0xff, 0x00, 
  0x00, 0xff, 0x00, 0x03, 0xfe, 0x0f, 0xf0, 0x7f, 0x80, 0x03, 0xfc, 0x30, 0x3f, 0xc0, 0xff, 0x00, 
  0x00, 0xff, 0x00, 0x01, 0xfe, 0x0f, 0xf0, 0x7f, 0x80, 0xc3, 0xfc, 0x00, 0x3f, 0xc0, 0xff, 0x00, 
  0x00, 0xff, 0x00, 0x21, 0xfe, 0x0f, 0xf0, 0x7f, 0x80, 0xc3, 0xfc, 0x06, 0x1f, 0xc0, 0xfe, 0x00, 
  0x00, 0x7f, 0x00, 0x31, 0xfe, 0x0f, 0xe0, 0x7f, 0x80, 0xc3, 0xfc, 0x06, 0x1f, 0xc0, 0xfe, 0x00, 
  0x00, 0x7f, 0x00, 0x70, 0xfe, 0x0f, 0xe0, 0x7f, 0x81, 0xc3, 0xfc, 0x0e, 0x0f, 0xc0, 0xfc, 0x00, 
  0x00, 0x3f, 0x00, 0x60, 0x7e, 0x0f, 0xc0, 0x7f, 0x81, 0xc3, 0xfc, 0x0e, 0x0f, 0xc0, 0xfc, 0x00, 
  0x00, 0x1f, 0x80, 0xc0, 0x7e, 0x0f, 0x80, 0x7f, 0x83, 0xc7, 0xfc, 0x3e, 0x07, 0xc0, 0xf8, 0x00, 
  0x00, 0x0f, 0x83, 0x80, 0x3e, 0x1f, 0x00, 0xff, 0x8f, 0xc7, 0xfc, 0xfe, 0x03, 0xe1, 0xf0, 0x00, 
  0x00, 0x07, 0xcf, 0x00, 0x0f, 0x3e, 0x01, 0xff, 0x9f, 0xcf, 0xfd, 0xfe, 0x01, 0xf3, 0xe0, 0x00, 
  0x00, 0x01, 0xcc, 0x00, 0x03, 0x38, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x33, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x1b, 0xdc, 0x77, 0x03, 0x30, 0x7c, 0x06, 0x70, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x13, 0xcc, 0xe3, 0x87, 0x18, 0x78, 0x0c, 0x30, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x13, 0xc5, 0xe3, 0x8f, 0x1c, 0x38, 0x1c, 0x10, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x13, 0xc5, 0xe3, 0xcf, 0x1c, 0x7c, 0x1e, 0x10, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x03, 0xc1, 0xe3, 0xdf, 0x1e, 0x7c, 0x1f, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x1f, 0x83, 0xc1, 0xe3, 0xdf, 0x1e, 0x3c, 0x1f, 0xe1, 0xf8, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x1f, 0x83, 0xc1, 0xe3, 0xdf, 0x1e, 0x3c, 0x0f, 0xf1, 0xf8, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x1f, 0x83, 0xc1, 0xe3, 0xdf, 0x1e, 0x7c, 0x07, 0xf9, 0xf8, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x03, 0xc1, 0xe3, 0xcf, 0x1e, 0x7c, 0x11, 0xf8, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x03, 0xc1, 0xe3, 0xcf, 0x1c, 0x7c, 0x50, 0x78, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x03, 0xc0, 0xe3, 0x8f, 0x1c, 0x7c, 0x58, 0x30, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x03, 0xc0, 0xe3, 0x07, 0x38, 0x7c, 0xdc, 0x70, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x07, 0xe0, 0x36, 0x03, 0xb0, 0x79, 0xde, 0xc0, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

void setup() {
  EEPROM.setMemPool(300, EEPROMSizeUno);
  Serial.begin(115200);
  addressPercentOn  = EEPROM.getAddress(sizeof(byte));
  addressLimitAngle = EEPROM.getAddress(sizeof(int));
  addressRollCorr   = EEPROM.getAddress(sizeof(float));
  addressPitchCorr  = EEPROM.getAddress(sizeof(float));

   // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }
  display.clearDisplay();
  display.setTextSize(0);
  display.setTextColor(SSD1306_WHITE); 
  
  if (!mpu.begin()) {
    display.print("Sensor init failed!");
    display.display();
    while (1)
      yield();
  }
  display.drawBitmap(0, 0, logo_bmp, 128, 64, 1);
  BotLeftString(softwareVersion);
  display.display();
  
  pixels.begin();
  unitsBtn.begin();
  upBtn.begin();
  downBtn.begin();
  
  //Load saved preferences or set defaults
  percentOn =EEPROM.readByte(addressPercentOn);
  limitAngle=EEPROM.readInt(addressLimitAngle);
  rollCorr  =EEPROM.readFloat(addressRollCorr);
  pitchCorr =EEPROM.readFloat(addressPitchCorr);

  if (percentOn == 255){
    percentOn = true;
    EEPROM.updateByte(addressPercentOn, percentOn);
  }
  if (limitAngle == 255){
    percentOn = 7;
    EEPROM.updateInt(addressPercentOn, percentOn);
  }
  if (rollCorr == 255){
    rollCorr = -0.19;
    EEPROM.updateFloat(addressPercentOn, rollCorr);
  }
  if (pitchCorr == 255){
    pitchCorr = -1.27;
    EEPROM.updateFloat(addressPercentOn, pitchCorr);
  }
  delay(3000); // Pause for 3 seconds 
  displayLimit();
  display.display();
  delay(3000); // Pause for 3 seconds 
}

void loop() {
  getButtonInput();
  avXg=0; avYg=0; avZg=0;
  for (int i=0; i < avg; i++){
  //  avXg=0; avYg=0; avZg=0;
    sensors_event_t a, g, temp;
    mpu.getEvent(&a, &g, &temp);    

    Xg=a.acceleration.x;
    Yg=a.acceleration.y;
    Zg=a.acceleration.z;
    avXg +=Xg;
    avYg +=Yg;
    avZg +=Zg;
  }
  Xg = avXg/avg;
  Yg = avYg/avg;
  Zg = avZg/avg;

  //Low Pass Filter jitter correction
  fXg = Xg * alpha + (fXg * (1.0 - alpha));
  fYg = Yg * alpha + (fYg * (1.0 - alpha));
  fZg = Zg * alpha + (fZg * (1.0 - alpha));

  rolld  = (atan2(-fYg, fXg)*180.0)/M_PI;
  pitchd = (atan2(fZg, sqrt(fYg*fYg + fXg*fXg))*180.0)/M_PI;

  rolld  = rolld + roll_corr;
  pitchd = pitchd + pitch_corr;

  led_index = round(rolld * 8/limitAngle) + 8;

  if (led_index < 0){
    led_index = 0; 
  }
  if (led_index > N_LEDS){
    led_index = N_LEDS-1; 
  }

  rollxten = rolld * 10.0 + 0.5;
  rollhundreds = abs((rollxten / 1000) % 10);
  rolltens = abs((rollxten / 100) % 10);
  rollones = abs((rollxten / 10) % 10);
  rolltenths = abs((rollxten) % 10);
  
  display.clearDisplay();
  display.setTextSize(5);
  
  //display.print(rollhundreds);
  if(rolltens!=0){
    display.setCursor(1,16);
    display.print(rolltens);
  }
  else display.setCursor(16,16);
  display.print(rollones);
  display.setTextSize(4);
  display.print(".");
  
  display.print(rolltenths);
  display.setTextSize(3);
  if (percentOn==false) display.print((char)247); // degree symbol 
  else display.print("%");
  display.display();

  if ((abs(rolld)>1*limitAngle/8)&&(abs(rolld)<4*limitAngle/8)) barcolor = g;
  if ((abs(rolld)>=4*limitAngle/8)&&(abs(rolld)<6*limitAngle/8)) barcolor = y;
  if ((abs(rolld)>=6*limitAngle/8)) barcolor = r;
  if (abs(rolld)>=limitAngle && millis()%2==1) barcolor = r;
  if (abs(rolld)>=limitAngle && millis()%2<1) barcolor = y;

  for (int i=0; i < N_LEDS; i++) {
  pixels.setPixelColor(i, off);
  }
  if (led_index==8){
    pixels.setPixelColor(led_index, g);
  }
  else {
  pixels.setPixelColor(8, w);
  if (led_index<8){
    for (int i=led_index; i < 8; i++){
    pixels.setPixelColor(i, barcolor);
    }
  }

  else {
    for (int i=led_index; i > 8; i--){
    pixels.setPixelColor(i, barcolor);
    }
  }
  
  }
  pixels.show();
  
  delay(delayval);
}

void getButtonInput() {
  if (unitsBtn.pressed()){
    unitsSettings();
  }
  if (upBtn.pressed()){
    limitSettings();
  }
  if (downBtn.pressed()){
    limitSettings();
  } 
}

void displayLimit(){
  display.clearDisplay();
  display.setTextSize(2);
  XCentreString("MAX",0);
  display.setTextSize(5);
  XCentreString(String(limitAngle), 20);
  display.setTextSize(4);
  if (percentOn==false){ // now using degrees
    display.print((char)247); // degree symbol
  }
  if (percentOn==true){ // now using percent
    display.print("%");
  }
}

void limitSettings(){
  settingsStartTime = millis();
  displayUnits();
  float percent = ((millis() - settingsStartTime)) / settingsTimeout;

  while (millis() - settingsStartTime <= settingsTimeout) {
    display.clearDisplay();
    if (unitsBtn.pressed()){
      settingsStartTime = millis();
      unitsSettings();
      settingsStartTime = millis();
    }   
    if (upBtn.pressed()){
      settingsStartTime = millis();
      if (limitAngle == 90) limitAngle = 1;
      else limitAngle += 1;
    }
    if (downBtn.pressed()){
      settingsStartTime = millis();
      if (limitAngle == 1) limitAngle = 90;
      else limitAngle -= 1;
    }
    percent = (millis() - settingsStartTime) / settingsTimeout;
    displayLimit();
    display.setTextSize(1);
    progressBar(percent);
    display.display();  
  }
  //preferences.putInt("memLimitAngle", limitAngle);
  EEPROM.updateInt(addressLimitAngle, limitAngle);
  //set the size of LED steps here?
}

void displayUnits(){
  display.clearDisplay();
  display.setTextSize(5);
  if (percentOn==false){ // now using degrees
    XYCentreString(String((char)247));
  } 
  if (percentOn==true){ // now using percent
    XYCentreString("%");
  }
}

void unitsSettings(){
  settingsStartTime = millis();
  displayUnits();
  float percent = ((millis() - settingsStartTime)) / settingsTimeout;
  
  while (millis() - settingsStartTime <= settingsTimeout) {
    display.clearDisplay();
    if (unitsBtn.pressed()){
      settingsStartTime = millis();
      percentOn = !percentOn;
    }
    percent = (millis() - settingsStartTime) / settingsTimeout;
    displayUnits();
    display.setTextSize(1);
    progressBar(percent);
    display.display();
  }
  //preferences.putBool("memPercentOn", percentOn);
  EEPROM.updateByte(addressPercentOn, percentOn);
}

void XCentreString(const String &buf, int y)
{
  int16_t x1, y1;
  uint16_t w, h;
  display.getTextBounds(buf, 0, 0, &x1, &y1, &w, &h); //calc width of new string
  display.setCursor(SCREEN_WIDTH/2 - w/2, y);
  display.print(buf);
}

void XYCentreString(const String &buf)
{
  int16_t x1, y1;
  uint16_t w, h;
  display.getTextBounds(buf, 0, 0, &x1, &y1, &w, &h); //calc width of new string
  display.setCursor(SCREEN_WIDTH/2 - w/2, SCREEN_HEIGHT/2 - h/2);
  display.print(buf);
}

void RightString(const String &buf, int y)
{
  int16_t x1, y1;
  uint16_t w, h;
  display.getTextBounds(buf, 0, 0, &x1, &y1, &w, &h); //calc width of new string
  display.setCursor(SCREEN_WIDTH - w, y);
  display.print(buf);
}

void BotRightString(const String &buf)
{
  int16_t x1, y1;
  uint16_t w, h;
  display.getTextBounds(buf, 0, 0, &x1, &y1, &w, &h); //calc width of new string
  display.setCursor(SCREEN_WIDTH - w, SCREEN_HEIGHT - h);
  display.print(buf);
}

void BotLeftString(const String &buf)
{
  int16_t x1, y1;
  uint16_t w, h;
  display.getTextBounds(buf, 0, 0, &x1, &y1, &w, &h); //calc width of new string
  display.setCursor(0, SCREEN_HEIGHT - h);
  display.print(buf);
}

void progressBar(float percent){
  display.drawRect(1, SCREEN_HEIGHT - 4, SCREEN_WIDTH - 1, 4, SSD1306_WHITE);
  display.fillRect(1, SCREEN_HEIGHT - 3, SCREEN_WIDTH * percent, 2, SSD1306_WHITE);
}