//v1.6
#include <EEPROM.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <OneButton.h>

// EEPROM
#define EEPROM_SIZE 512
#define EEPROM_RPM_LIMIT_ADDR sizeof(Mode)
#define EEPROM_THROTTLE_LIMIT_ADDR sizeof(Mode)
#define EEPROM_MODE_ADDR 0

#define SCL_PIN 1
#define SDA_PIN 2
#define HIGH_BEAM_BUTTON_PIN 40

#define SCREEN_WIDTH 128 // OLED width,  in pixels
#define SCREEN_HEIGHT 64  // OLED height, in pixels

// Enum for Mode
enum Mode {
  AUTO,
  RACE
};

Mode currentMode;

int CAN_RPM_LIMIT;
int selectedRpmLimit = CAN_RPM_LIMIT;

int CAN_THROTTLE_LIMIT;
int selectedThrottleLimit = CAN_THROTTLE_LIMIT;

int selectedMode;

long lastmillis = 0;
long maxtime = 30000;

bool highBeamActive = false;
bool rpmChangeMenu = false;

bool highBeamButtonReleased = true;
unsigned long buttonPressStartTime = 0;
const unsigned long longPressDuration = 2000;

const int NUM_ITEMS = 3;
const int MAX_ITEM_LENGTH = 20;

char menu_items[NUM_ITEMS] [MAX_ITEM_LENGTH] = {
  {"RPM LIMIT"},
  {"THROTTLE LIMIT"},
  {"BOOT MODE"}
};

int item_selected = 0;

int item_sel_previous;
int item_sel_next;

int current_screen = 0;   // 0 = dashboard, 1 = menu, 2 = change

OneButton button(HIGH_BEAM_BUTTON_PIN, true);

// create an OLED display object connected to I2C
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1);

// 'Untitled', 164x64px
const unsigned char epd_bitmap_bootscreen [] 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, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x0c, 0x07, 0xf0, 0xfe, 0x04, 0x30, 0x0c, 0x0e, 0x00, 0x03, 0xfe, 0xc0, 0x30, 0x7e, 0x00,
  0x00, 0x0e, 0x06, 0x38, 0xc7, 0x04, 0x30, 0x0c, 0x0e, 0x00, 0x03, 0x00, 0x60, 0x31, 0xc3, 0x00,
  0x00, 0x1e, 0x06, 0x18, 0xc3, 0x04, 0x30, 0x0c, 0x0a, 0x00, 0x03, 0x00, 0x60, 0x63, 0x80, 0x00,
  0x00, 0x1b, 0x06, 0x0c, 0xc1, 0x04, 0x30, 0x0c, 0x1b, 0x00, 0x03, 0x00, 0x20, 0x63, 0x00, 0x00,
  0x00, 0x33, 0x06, 0x08, 0xc3, 0x04, 0x30, 0x0c, 0x11, 0x00, 0x03, 0x00, 0x30, 0x43, 0x00, 0x00,
  0x00, 0x31, 0x06, 0x18, 0xc3, 0x04, 0x30, 0x0c, 0x31, 0x80, 0x03, 0xfc, 0x30, 0xc6, 0x00, 0x00,
  0x00, 0x21, 0x87, 0xf8, 0xfe, 0x04, 0x30, 0x0c, 0x31, 0x80, 0x03, 0xfc, 0x18, 0xc6, 0x00, 0x00,
  0x00, 0x61, 0x87, 0xe0, 0xde, 0x04, 0x30, 0x0c, 0x20, 0x80, 0x03, 0x00, 0x18, 0x82, 0x00, 0x00,
  0x00, 0x7f, 0x86, 0x00, 0xc3, 0x04, 0x30, 0x0c, 0x7f, 0xc0, 0x03, 0x00, 0x19, 0x83, 0x00, 0x00,
  0x00, 0x7f, 0xc6, 0x00, 0xc3, 0x04, 0x30, 0x0c, 0x7f, 0xc0, 0x03, 0x00, 0x0d, 0x83, 0x00, 0x00,
  0x00, 0xc0, 0xc6, 0x00, 0xc1, 0x84, 0x30, 0x0c, 0xc0, 0x60, 0x03, 0x00, 0x0f, 0x01, 0x80, 0x00,
  0x00, 0xc0, 0x66, 0x00, 0xc0, 0x84, 0x30, 0x0c, 0xc0, 0x60, 0x03, 0x80, 0x07, 0x01, 0xc3, 0x00,
  0x01, 0x80, 0x66, 0x00, 0xc0, 0xc4, 0x3f, 0xcc, 0xc0, 0x20, 0x03, 0xfe, 0x06, 0x00, 0x7e, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0f, 0xf8, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x7e, 0x01, 0x80, 0xe0, 0x70, 0xfe, 0x07, 0xe0, 0x7f, 0x80, 0x06, 0x03, 0xf8, 0x7e, 0x00,
  0x00, 0xc2, 0x03, 0x80, 0xe0, 0xf1, 0xc2, 0x0c, 0x30, 0x60, 0x00, 0x07, 0x03, 0x1c, 0x63, 0x80,
  0x01, 0x80, 0x02, 0xc0, 0xe0, 0xf1, 0x80, 0x18, 0x18, 0x60, 0x00, 0x0d, 0x03, 0x0c, 0x61, 0x80,
  0x01, 0x80, 0x06, 0xc0, 0xb0, 0xb1, 0x00, 0x30, 0x0c, 0x60, 0x00, 0x0d, 0x83, 0x06, 0x60, 0x80,
  0x01, 0x80, 0x06, 0x40, 0x91, 0xb1, 0x80, 0x30, 0x0c, 0x60, 0x00, 0x09, 0x83, 0x06, 0x60, 0x80,
  0x00, 0xf0, 0x04, 0x60, 0x91, 0x31, 0xf0, 0x30, 0x0c, 0x7f, 0x80, 0x18, 0x83, 0x0c, 0x61, 0x80,
  0x00, 0x7e, 0x0c, 0x20, 0x99, 0x30, 0x7e, 0x30, 0x0c, 0x7f, 0x80, 0x18, 0xc3, 0xfc, 0x7f, 0x80,
  0x00, 0x07, 0x0c, 0x30, 0x8b, 0x30, 0x06, 0x30, 0x0c, 0x60, 0x00, 0x30, 0xc3, 0xf0, 0x7e, 0x00,
  0x00, 0x03, 0x1f, 0xf0, 0x8e, 0x30, 0x03, 0x30, 0x0c, 0x60, 0x00, 0x3f, 0xe3, 0x00, 0x60, 0x00,
  0x00, 0x03, 0x1f, 0xf0, 0x8e, 0x30, 0x03, 0x10, 0x08, 0x60, 0x00, 0x3f, 0xe3, 0x00, 0x60, 0x00,
  0x00, 0x03, 0x18, 0x18, 0x80, 0x30, 0x03, 0x18, 0x18, 0x60, 0x00, 0x60, 0x23, 0x00, 0x60, 0x00,
  0x01, 0x87, 0x30, 0x18, 0x80, 0x31, 0x86, 0x0e, 0x70, 0x60, 0x0c, 0x60, 0x33, 0x00, 0x60, 0x00,
  0x01, 0xfc, 0x30, 0x0c, 0x80, 0x31, 0xfc, 0x07, 0xe0, 0x7f, 0x8c, 0xc0, 0x33, 0x00, 0x60, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};

// 'selection-16', 128x18px
const unsigned char selection [] PROGMEM = {
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01,
  0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff
};

// 'gauge-16', 16x16px
const unsigned char epd_bitmap_gauge_16 [] PROGMEM = {
  0x00, 0x00, 0x03, 0xc0, 0x08, 0x10, 0x10, 0x08, 0x20, 0x04, 0x60, 0x06, 0x40, 0x12, 0x01, 0xe0,
  0x42, 0x42, 0x01, 0x80, 0x40, 0x02, 0x40, 0x02, 0x20, 0x04, 0x0c, 0x30, 0x07, 0xe0, 0x00, 0x00
};
// 'throttle-16', 16x16px
const unsigned char epd_bitmap_throttle_16 [] PROGMEM = {
  0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3c, 0x00, 0x72, 0x00, 0xd3, 0xfe, 0xb3, 0xff,
  0x8b, 0x03, 0x8b, 0x7d, 0x0b, 0x00, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00
};
// 'gear-16', 16x16px
const unsigned char epd_bitmap_gear_16 [] PROGMEM = {
  0x00, 0x00, 0x01, 0x80, 0x0c, 0x30, 0x10, 0x08, 0x27, 0xe4, 0x25, 0xa4, 0x0a, 0x50, 0x44, 0x22,
  0x44, 0x22, 0x0a, 0x50, 0x25, 0xa4, 0x27, 0xe4, 0x00, 0x08, 0x0c, 0x30, 0x01, 0x80, 0x00, 0x00
};

const unsigned char* bitmap_icons[5] = {
  epd_bitmap_gauge_16,
  epd_bitmap_throttle_16,
  epd_bitmap_gear_16  
};

void header() {
  display.setTextSize(1);
  display.setTextColor(WHITE);
  display.setCursor(0, 0);
  display.print("      APRILIA_EVC");
  display.drawLine(0, 9, 128, 9, WHITE);
}

void refresh() {
  display.display();
  delay(00);
  display.clearDisplay();
}

void dashboard() {
  int receivedRPMValue = 10000;
  int receivedThrottleValue = 80;
  int receivedEVPosValue = 3000;

  display.clearDisplay();
  header();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  display.setCursor(4, 11);
  display.print("RPM: ");
  display.print(receivedRPMValue); // Replace with actual RPM value

  display.setCursor(4, 20);
  display.print("THR: ");
  display.print(receivedThrottleValue); // Replace with actual throttle value

  display.setCursor(4, 29);
  display.print("EVPos: ");
  display.print(receivedEVPosValue); // Replace with actual EVPos value

  display.setCursor(4, 38);
  display.print("Mode: ");
  display.print((currentMode == Mode::AUTO) ? "AUTO" : "RACE");

  display.setCursor(4, 47);
  display.print("RPMLIMIT: ");
  display.print(selectedRpmLimit); // Replace with actual RPM value

  display.setCursor(4, 56);
  display.print("THRLIMIT: ");
  display.print(selectedThrottleLimit); // Replace with actual RPM value
  refresh();
}

void menu() {
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);

  header();

  // Prev item
  display.drawBitmap(3, 11, bitmap_icons[item_sel_previous], 16, 16, 1);
  display.setCursor(25, 14);
  display.println(menu_items[item_sel_previous]);

  // Sel item
  display.drawBitmap(3, 30, bitmap_icons[item_selected], 16, 16, 1);
  display.setCursor(25, 33);
  display.println(menu_items[item_selected]);

  // Next item
  display.drawBitmap(3, 47, bitmap_icons[item_sel_next], 16, 16, 1);
  display.setCursor(25, 50);
  display.println(menu_items[item_sel_next]);

  // Selection bar
  display.drawBitmap(0, 29, selection, 128, 18, 1);

  display.display();
}

void change() {
  if (item_selected == 0 && current_screen == 2) {
    display.clearDisplay();
    header();
    display.setCursor(0, 11);
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.print("SET RPM LIMIT");
    display.setCursor(0, 20);
    display.print(">RPMLIMIT:");
    display.setCursor(90, 20);
    display.print(selectedRpmLimit);
    display.display();
  }
  if (item_selected == 1 && current_screen == 2) {
    display.clearDisplay();
    header();
    display.setCursor(0, 11);
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.print("SET THROTTLE LIMIT");
    display.setCursor(0, 20);
    display.print(">THROTTLELIMIT:");
    display.setCursor(90, 20);
    display.print(selectedThrottleLimit);
    display.display();
  }
  if (item_selected == 2 && current_screen == 2) {
    display.clearDisplay();
    header();
    display.setCursor(0, 11);
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.print("SET BOOT MODE");
    display.setCursor(0, 20);
    display.print(">MODE:");
    display.setCursor(90, 20);
    display.print(currentMode);
    display.display();
  }
}

void click(void *oneButton) {
  Serial.println("\t - Singleclick()");
  if (current_screen == 0) {
    // Change mode for exhaust valve
  }
  if (current_screen == 1) {
    item_selected = item_selected + 1;
    if (item_selected >= NUM_ITEMS) {
      item_selected = 0;
    }
  }

  if (current_screen == 2 && item_selected == 0) {
    selectedRpmLimit += 500;
    if (selectedRpmLimit > 7000) {
      selectedRpmLimit = 2000;
    }
  }
  else if (current_screen == 2 && item_selected == 1) {
    selectedThrottleLimit += 5;
    if (selectedThrottleLimit > 30) {
      selectedThrottleLimit = 10;
    }
  }
  else if (current_screen == 2 && item_selected == 2) {
    // Change Mode functionality
  }
}

void doubleclick(void *oneButton) {
  Serial.println("\t - Doubleclick()");
  if (current_screen == 0) {
    // Change mode for exhaust valve
  }
  if (current_screen == 1) {
    item_selected = item_selected - 1;
    if (item_selected < 0) {
      item_selected = NUM_ITEMS - 1;
    }
  }
  if (current_screen == 2 && item_selected == 0) {
    selectedRpmLimit -= 500;
    if (selectedRpmLimit < 2000) {
      selectedRpmLimit = 7000;
    }
  }
  else if (current_screen == 2 && item_selected == 1) {
    selectedThrottleLimit -= 5;
    if (selectedThrottleLimit < 10) {
      selectedThrottleLimit = 30;
    }
  }
  else if (current_screen == 2 && item_selected == 2) {
    // Change Mode functionality
  }
}

void LongPressStart(void *oneButton)
{
  Serial.println("\t - LongPressStart()");
  if (current_screen == 0) {
    current_screen = 1;
  }
  else if (current_screen == 1) {
    current_screen = 2;
  }
  else {
    if (current_screen = 2 && item_selected == 0) {
      CAN_RPM_LIMIT = selectedRpmLimit;
      EEPROM.put(EEPROM_RPM_LIMIT_ADDR, CAN_RPM_LIMIT);
      EEPROM.commit();
    }
    if (current_screen = 2 && item_selected == 1) {
      CAN_THROTTLE_LIMIT = selectedThrottleLimit;
      EEPROM.put(EEPROM_THROTTLE_LIMIT_ADDR, CAN_THROTTLE_LIMIT);
      EEPROM.commit();
    }
    if (current_screen = 2 && item_selected == 2) {
      //currentMode = currentMode + 1;
      EEPROM.put(EEPROM_MODE_ADDR, currentMode);
      EEPROM.commit();
    }
    current_screen = 0;
  }
}

void DuringLongPress(void *oneButton)
{
  Serial.println("\t - DuringLongPress()");
}

void LongPressStop(void *oneButton)
{
  Serial.println("\t - LongPressStop()\n");
}

void setup() {
  Serial.begin(115200);

  pinMode(HIGH_BEAM_BUTTON_PIN, INPUT_PULLUP);  // Initialize button pins

  EEPROM.begin(EEPROM_SIZE);  // Initialize EEPROM

  EEPROM.get(EEPROM_RPM_LIMIT_ADDR, CAN_RPM_LIMIT);
  EEPROM.get(EEPROM_THROTTLE_LIMIT_ADDR, CAN_THROTTLE_LIMIT);
  EEPROM.get(EEPROM_MODE_ADDR, currentMode);

  selectedRpmLimit = CAN_RPM_LIMIT;
  selectedThrottleLimit = CAN_THROTTLE_LIMIT;
  //currentMode = currentMode

  // TEST bacause EEPROM not working in Wokwi
  selectedRpmLimit = 5500;
  selectedThrottleLimit = 14;
  //currentMode = Mode::Auto;

  Wire.begin(SDA_PIN, SCL_PIN);  // Initialize I2C communication


  if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3c)) {   // Initialize display
    Serial.println(F("SSD1306 allocation failed"));
    while (1);
  }

  display.clearDisplay();
  display.drawBitmap(0, 0, epd_bitmap_bootscreen, 128, 64, 1);   //Bootlogo
  display.display();
  delay(3000);

  button.attachClick(click, &button);
  button.attachDoubleClick(doubleclick, &button);
  button.attachLongPressStart(LongPressStart, &button);
  button.attachDuringLongPress(DuringLongPress, &button);
  button.attachLongPressStop(LongPressStop, &button);
  //button.setLongPressIntervalMs(1000);
}

void loop() {
  button.tick();

  item_sel_previous = item_selected - 1;
  if (item_sel_previous < 0) {
    item_sel_previous = NUM_ITEMS - 1; // previous item would be below first = make it the last
  }
  item_sel_next = item_selected + 1;
  if (item_sel_next >= NUM_ITEMS) {
    item_sel_next = 0; // next item would be after last = make it the first
  }

  if (current_screen == 0) { // DASHBOARD SCREEN
    dashboard();
  }
  else if (current_screen == 1) { // MENU SCREEN
    menu();
  }
  else if (current_screen == 2) { // CHANGE SCREEN
    change();
  }
}