// Arduino Configurations:

#include <LiquidCrystal_I2C.h>
#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#define SCREEN_WIDTH 128 // OLED display width, in pixels
#define SCREEN_HEIGHT 64 // OLED display height, in pixels
const int EthanolSensorPin = A0;
const int FuelTemperatureSensorPin = A1;
const int PedalThrottleSensorPin = A2;
const int IgnitionPin = 6;
const int BC_ButtonPin = 7;
const int RES_ButtonPin = 8;
const int LongDown_ButtonPin = 9;
const int ShortDown_ButtonPin = 10;
const int ShortUp_ButtonPin = 11;
const int LongUp_ButtonPin = 12;
const int DTC_LEDPin = 13;
bool IgnitionOn = false;
bool IgnitionOn_old = false;
bool DebugBufferFull = false;
String DebugBuffer = "";
LiquidCrystal_I2C lcd(0x27, 20, 4);
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, 4);
uint8_t flag_rootMenuDTC_old = 0;
uint8_t flag_subMenuDTC_old = 0;
uint8_t flag_mapswitchDTC_old = 0;
uint8_t flag_maxCoolingDTC_old = 0;
uint8_t flag_burbleDTC_old = 0;
uint8_t flag_AntilagDTC_old = 0;
uint8_t flag_SelectAntilagDTC_old = 0;
uint8_t flag_ethanolContentDisplayDTC_old = 0;
uint8_t flag_EthanolContentOverrideSelectionsDTC_old = 0;
uint8_t flag_EthanolContentOverrideToggleDTC_old = 0;
uint8_t flag_ExhaustFlapSelectionsDTC_old = 0;
uint8_t flag_ValetSelectionsDTC_old = 0;
uint8_t flag_DeveloperSelectionsDTC_old = 0;
uint8_t ValetExceededLimits_old = 0;
uint8_t LedStatus_Old = 0;
uint16_t Com_Tx_DispRpmEngCrsh_Old = 0;

#define BitmapWidth 82
#define BitmapHeight 64
static const unsigned char PROGMEM EngineLightBitmap[] =
{
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b01111111,0b11111111,0b11111111,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b11111111,0b11111111,0b11111111,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b01111111,0b11111111,
	0b11111111,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b01111111,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00111100,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b01111111,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b11111111,0b11111111,0b11111111,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000001,0b11111111,0b11111111,0b11111111,0b11000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000001,0b11111111,0b11111111,0b11111111,0b11000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000001,0b11100000,0b00000000,0b00000001,0b11000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000110,0b00011111,0b11100000,0b00000000,0b00000001,0b11000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00001111,0b00111111,0b11100000,0b00000000,0b00000001,
	0b11111111,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00001111,0b00111111,0b11000000,0b00000000,
	0b00000001,0b11111111,0b11000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00001111,0b00111000,0b00000000,
	0b00000000,0b00000001,0b11111111,0b11000111,0b11000000,0b00000000,0b00000000,0b00000000,0b00001111,0b00111000,
	0b00000000,0b00000000,0b00000000,0b11111111,0b11001111,0b11100000,0b00000000,0b00000000,0b00000000,0b00001111,
	0b00111000,0b00000000,0b00000000,0b00000000,0b00000011,0b11011111,0b11110000,0b00000000,0b00000000,0b00000000,
	0b00001111,0b00111000,0b00000000,0b00000000,0b00000000,0b00000011,0b11011100,0b01110000,0b00000000,0b00000000,
	0b00000000,0b00001111,0b00111000,0b00000000,0b00000000,0b00000000,0b00000011,0b11011100,0b01110000,0b00000000,
	0b00000000,0b00000000,0b00001111,0b00111000,0b00000000,0b00000000,0b00000000,0b00000011,0b11111100,0b01110000,
	0b00000000,0b00000000,0b00000000,0b00001111,0b00111000,0b00000000,0b00000000,0b00000000,0b00000011,0b11111100,
	0b01111000,0b00000000,0b00000000,0b00000000,0b00001111,0b11111000,0b00000000,0b00000000,0b00000000,0b00000001,
	0b11111000,0b01111000,0b00000000,0b00000000,0b00000000,0b00001111,0b11111000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b01111000,0b00000000,0b00000000,0b00000000,0b00001111,0b11111000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b01111000,0b00000000,0b00000000,0b00000000,0b00001111,0b00111000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b01111000,0b00000000,0b00000000,0b00000000,0b00001111,0b00111000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b01111000,0b00000000,0b00000000,0b00000000,0b00001111,
	0b00111000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b01111000,0b00000000,0b00000000,0b00000000,
	0b00001111,0b00111000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b01111000,0b00000000,0b00000000,
	0b00000000,0b00001111,0b00111000,0b00000000,0b00000000,0b00000000,0b00000001,0b11111000,0b01111000,0b00000000,
	0b00000000,0b00000000,0b00001111,0b00111000,0b00000000,0b00000000,0b00000000,0b00000011,0b11111100,0b01111000,
	0b00000000,0b00000000,0b00000000,0b00001111,0b00111000,0b00000000,0b00000000,0b00000000,0b00000011,0b11111100,
	0b01111000,0b00000000,0b00000000,0b00000000,0b00001111,0b00111111,0b11111110,0b00000000,0b00000000,0b00000011,
	0b11111100,0b01110000,0b00000000,0b00000000,0b00000000,0b00001111,0b00111111,0b11111111,0b10000000,0b00000000,
	0b00000011,0b11011100,0b01110000,0b00000000,0b00000000,0b00000000,0b00000110,0b00011111,0b11111111,0b11000000,
	0b00000000,0b00000011,0b11011111,0b11110000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000111,
	0b11110000,0b00000000,0b00000011,0b11011111,0b11110000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000001,0b11111000,0b00000000,0b00000011,0b11001111,0b11100000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b01111111,0b11111111,0b11111111,0b11000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00111111,0b11111111,0b11111111,0b11000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00001111,0b11111111,0b11111111,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000
};
static const unsigned char PROGMEM MgflasherBitmap[] =
{
	0b00000000,0b00000000,0b00000000,0b00000000,0b00001111,0b11000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00111111,0b00000000,0b00000000,0b00000000,0b00000000,0b11111111,0b11111000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00111111,0b00000000,0b00000000,0b00000000,0b00000011,0b11000000,0b00001110,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,0b00000000,0b00001110,0b00000011,0b00000001,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,0b00000000,0b00111000,0b00111111,0b10111000,
	0b11000000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,0b00000000,0b01110001,0b01111111,
	0b10111110,0b00100000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,0b00000000,0b11000111,
	0b00111111,0b00111111,0b10000000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,0b00000001,
	0b10001111,0b10000000,0b00001111,0b11000000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,
	0b00000011,0b00111111,0b00000000,0b00000011,0b10110000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,
	0b00000000,0b00000110,0b01111100,0b01100000,0b00000001,0b01110000,0b00000000,0b00000000,0b00000000,0b00111111,
	0b00000000,0b00000000,0b00001100,0b01111001,0b10000000,0b00000000,0b11111000,0b00000000,0b00000000,0b00000000,
	0b00111111,0b00000000,0b00000000,0b00011001,0b10110010,0b00000000,0b00000000,0b01111100,0b00000000,0b00000000,
	0b00000000,0b00111111,0b00000000,0b00000000,0b00110011,0b10000100,0b00000000,0b00000000,0b00111100,0b00000000,
	0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,0b00110111,0b10011000,0b00000000,0b00000000,0b00111110,
	0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,0b01100111,0b10110000,0b00000000,0b00000000,
	0b00011110,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,0b11001111,0b00100000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,0b11011110,0b01000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000001,0b10011100,
	0b10000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000001,
	0b10111101,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,
	0b00000011,0b00011001,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00111111,
	0b00000000,0b00000011,0b01011010,0b00000001,0b11111100,0b00000000,0b11111100,0b01111111,0b11111100,0b00000000,
	0b00111111,0b00000000,0b00000110,0b01100000,0b00000001,0b11111100,0b00000001,0b11111101,0b11111111,0b11111110,
	0b00000000,0b00111111,0b00000000,0b00000110,0b11100100,0b00000001,0b11111110,0b00000011,0b11111001,0b11111111,
	0b11111111,0b00000000,0b00111111,0b00000000,0b00000100,0b11100000,0b00000001,0b11111110,0b00000011,0b11111011,
	0b11111111,0b11111111,0b00000000,0b00111111,0b00000000,0b00001100,0b11001000,0b00000011,0b11111110,0b00000111,
	0b11111011,0b11111111,0b11111111,0b00000000,0b00111111,0b00000000,0b00001101,0b11001000,0b00000011,0b11111110,
	0b00000111,0b11110011,0b11110000,0b01111110,0b00000000,0b00111111,0b00000000,0b00001001,0b11010000,0b00000011,
	0b11111111,0b00001111,0b11110011,0b11110000,0b01111110,0b00000000,0b00111111,0b00000000,0b00011001,0b10010000,
	0b00000011,0b11111111,0b00001111,0b11110111,0b11110000,0b01111110,0b00000000,0b00111111,0b00000000,0b00011011,
	0b10000000,0b00000011,0b11111111,0b00011111,0b11110111,0b11100000,0b00000000,0b00000000,0b00111111,0b00000000,
	0b00011011,0b10100000,0b00000111,0b11111111,0b00011111,0b11110111,0b11100000,0b00000000,0b00000000,0b00111111,
	0b00000000,0b00010011,0b00100000,0b00000111,0b11111111,0b00111111,0b11110111,0b11100111,0b11111110,0b00000000,
	0b00111111,0b00000000,0b00010001,0b00100000,0b00000111,0b11101111,0b11111101,0b11100111,0b11100111,0b11111100,
	0b00000000,0b00111111,0b00000000,0b00010010,0b01000000,0b00000111,0b11101111,0b11111101,0b11100111,0b11100111,
	0b11111100,0b00000000,0b00111111,0b00000000,0b00110111,0b01000000,0b00000111,0b11101111,0b11111001,0b11101111,
	0b11000111,0b11111100,0b00000000,0b00111111,0b00000000,0b00110111,0b01000000,0b00000111,0b11101111,0b11111001,
	0b11101111,0b11000111,0b11111100,0b00000000,0b00111111,0b00000000,0b00110110,0b01000000,0b00001111,0b11000111,
	0b11110011,0b11101111,0b11000000,0b11111100,0b00000000,0b00111111,0b00000000,0b00110110,0b01000000,0b00001111,
	0b11000111,0b11110011,0b11001111,0b11000001,0b11111100,0b00000000,0b00111111,0b00000000,0b00110110,0b01000000,
	0b00001111,0b11000111,0b11100011,0b11001111,0b11000001,0b11111000,0b00000000,0b00111111,0b00000000,0b00110110,
	0b00000000,0b00001111,0b11000111,0b11100011,0b11001111,0b11111111,0b11111000,0b00000000,0b00111111,0b00000000,
	0b00110110,0b00000000,0b00001111,0b11000011,0b11000011,0b11011111,0b11111111,0b11111000,0b00000000,0b00111111,
	0b00000000,0b00110110,0b00000000,0b00001111,0b10000000,0b00000011,0b11011111,0b11111111,0b11111000,0b00000000,
	0b00111111,0b00000000,0b00110110,0b00000000,0b00011111,0b10000000,0b00000111,0b11001111,0b11111111,0b11110000,
	0b00000000,0b00111111,0b00000000,0b00010010,0b00000000,0b00011111,0b10000000,0b00000111,0b11101111,0b11111111,
	0b11100000,0b00000000,0b00111111,0b00000000,0b00010000,0b01000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00010010,0b01000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00011010,0b01000000,0b00011110,0b01000001,
	0b11000111,0b10110011,0b01111001,0b11100000,0b00000000,0b00111111,0b00000000,0b00011011,0b01000000,0b00011110,
	0b11000001,0b11001111,0b10110011,0b01111001,0b11110000,0b00000000,0b00111111,0b00000000,0b00001011,0b01000000,
	0b00011000,0b11000011,0b11001100,0b00110110,0b01000011,0b00110000,0b00000000,0b00111111,0b00000000,0b00001001,
	0b00100000,0b00011110,0b11000010,0b01001111,0b00111110,0b01111011,0b11100000,0b00000000,0b00111111,0b00000000,
	0b00001101,0b00100000,0b00111100,0b11000111,0b11000011,0b00110110,0b11000011,0b11100000,0b00000000,0b00111111,
	0b00000000,0b00000101,0b10100000,0b00110000,0b11000111,0b11011011,0b01100110,0b11000010,0b01000000,0b00000000,
	0b00111111,0b00000000,0b00000110,0b10010000,0b00110001,0b11111100,0b01111111,0b01100110,0b11111010,0b01100000,
	0b00000000,0b00111111,0b00000000,0b00000010,0b00010000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00111111,0b00000000,0b00000011,0b00001000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000001,0b00100100,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000001,0b10100100,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,0b11010011,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,0b01001000,
	0b10000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,
	0b00100100,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00111111,0b00000000,
	0b00000000,0b00011000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00111111,
	0b00000000,0b00000000,0b00001100,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00111111,0b00000000,0b00000000,0b00000011,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00111111,0b00000000,0b00000000,0b00000000,0b11000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00111111,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,0b00000000,
	0b00000000,0b00000000,0b00000000,0b00111111
};

// ECU OEM RAM items
uint16_t Com_Tx_DispRpmEngCrsh = 65535;
int16_t Epm_nEng = 3000;
uint8_t MoFAPP_r = 0;
uint16_t Ps = 0;
uint8_t T15_stRaw = 0;
uint16_t Tabg_vl_xab = 0;
uint8_t tans_msg = 0;
uint16_t Tmot = 90;
uint16_t tnst_w_msg = 0;

// ECU custom code RAM items
uint16_t Antilag_ActivationDelay = 200;
uint16_t Antilag_CoolantTempMax = 120;
uint16_t Antilag_CoolantTempMin = 75;
uint16_t Antilag_EgtTempMax = 1000;
uint8_t Antilag_IatTempMax = 120;
uint16_t Antilag_MaxDuration = 2;
uint16_t Antilag_OverBoostThreshold = 300;
uint8_t Antilag_Pedal = 5;
uint16_t Antilag_RollingSpeedThreshold = 10;
uint16_t Antilag_RollRpmMax = 6000;
uint16_t Antilag_RollRpmMin = 2000;
uint16_t Antilag_RpmDelayUntilCutRoll = 25;
uint16_t Antilag_RpmDelayUntilCutStand = 25;
uint16_t Antilag_TargetBoostRoll = 1600;
uint16_t Antilag_TargetBoostStand = 1400;
uint16_t Antilag_timeElapsed = 0;
uint16_t AntilagRPM_EEP = 3000;
uint16_t AntilagCounter = 65535;
uint16_t antilagRPMSetpoint = 3000;
uint16_t antilagRPMSetpointSave = 3000;
uint8_t AntilagSafetyShutdown = 0;
uint8_t antilagStatus = 0;
uint16_t B_ccDownLong = 0;
uint16_t B_ccDownShort = 0;
uint16_t B_ccDownShort_LastStatus = 0;
uint16_t B_ccResume = 0;
uint16_t B_ccUpLong = 0;
uint16_t B_ccUpShort = 0;
uint16_t B_ccUpShort_LastStatus = 0;
uint8_t B_ts_BC = 0;
uint8_t BlinkDuration = 0;
uint8_t BlinkRest = 0;
uint8_t bufferTemp = 0;
uint8_t BurbleAggressionLevel_EEP = 0;
uint16_t cancelationTimer = 0;
uint8_t cancelationTimerisSet = 0;
uint16_t CelLastMillis10ms = 0;
uint8_t checksumInvalid = 0;
uint8_t checksumValActual = 0;
uint8_t checksumValSaved = 0;
uint16_t Com_Rx_VVehCog = 0;
uint8_t ControlLed = 0;
uint8_t currentGaugePosition = 0;
uint16_t CurrentNumberOfBlinks = 0;
uint8_t custom_burble_status = 0;
uint8_t CW_Antilag = 1;
uint8_t CW_BOTF = 1;
uint8_t CW_CCMenu = 1;
uint8_t CW_ExhaustFlap = 1;
uint8_t CW_FlexFuel = 1;
uint8_t CW_FlexFuelBlending = 1;
uint8_t CW_IsDeveloper = 0;
uint8_t CW_max_cooling = 1;
uint8_t CW_OctaneSwitch = 1;
uint8_t CW_SensorEthanol = 1; // is a sensor installed? 
uint8_t CW_Valet = 1;
uint8_t downPressed = 0;
uint8_t eepDelayFinished = 0;
uint32_t EepromBuffer_SA = 0; // array of 16  
uint8_t EepromChecked = 0;
uint8_t EepromCustomCodeByte0 = 0;
uint8_t EepromCustomCodeByte1 = 0;
uint8_t EepromCustomCodeByte2 = 0;
uint8_t EepromCustomCodeByte3 = 0;
uint8_t EepromCustomCodeByte4 = 0;
uint8_t EepromCustomCodeByte5 = 0;
uint8_t EepromCustomCodeByte6 = 0;
uint8_t EepromLoadedCorrectly = 0;
uint8_t EepromWriteRequested = 0;
uint16_t ExhaustFlapClosedMaxLoad = 120;
uint8_t ExhaustFlapValue = 0;
uint8_t ExhaustFlapValue_EEP = 0;
uint16_t FastBlinks = 9696;
uint8_t firstCut = 0;
uint8_t flag_AntilagDTC = 0;
uint8_t flag_burbleDTC = 0;
uint8_t flag_DeveloperSelectionsDTC = 0;
uint8_t flag_ethanolContentDisplayDTC = 0;
uint8_t flag_EthanolContentOverrideSelectionsDTC = 0;
uint8_t flag_EthanolContentOverrideToggleDTC = 0;
uint8_t flag_ExhaustFlapSelectionsDTC = 0;
uint8_t flag_mapswitchDTC = 0;
uint8_t flag_maxCoolingDTC = 0;
uint8_t flag_rootMenuDTC = 0;
uint8_t flag_SelectAntilagDTC = 0;
uint8_t flag_subMenuDTC = 0;
uint8_t flag_ValetSelectionsDTC = 0;
uint8_t flagBC = 0;
uint8_t flagDynamicAntilagRPM = 0;
uint8_t FlexFuelBlendingEnabled = CW_FlexFuelBlending;
uint8_t FlexFuelBufferEthanolContent[7]{ 0, 0, 0, 0, 0, 0, 0 };
uint8_t FlexFuelBufferEthanolContentPointerNextValue = 0;
uint8_t FlexFuelBufferEthanolContentPointerValue = 0;
uint8_t FlexFuelBufferFuelTemperature[7]{ 0, 0, 0, 0, 0, 0, 0 };
uint8_t FlexFuelBufferFuelTemperaturePointerNextValue = 0;
uint8_t FlexFuelBufferFuelTemperaturePointerValue = 0;
uint8_t FlexFuelBufferIndex = 0;
uint8_t FlexFuelEthanolContentOverrideFlag = 0;
uint8_t FlexFuelEthanolContentOverrideFlag_EEP = 0;
uint16_t FlexFuelEthanolContentOverrideValue = 0;
uint16_t FlexFuelEthanolContentOverrideValue_EEP = 0;
uint16_t FlexFuelEthanolContentValue = 0;
uint16_t FlexFuelEthanolDisplayActivationDelay = 50; // 0.5s    
uint8_t FlexFuelSensorEthanolRawValue = 0; // ethanol data as percentage 
uint8_t FlexFuelSensorStatusRawValue = 0; // 0x00 == Sensor OK, 
uint8_t FlexFuelSensorTemperatureRawValue = 0; // temperature data as Celsius 
int16_t FlexFuelTemperatureValue = 0;
uint32_t GlobalTimerForEngineRunning32Bit10msResolution = 0;
uint8_t InvalidSelectionMade = 0;
uint32_t InvalidSelectionMadeLastMillis10ms = 0;
uint8_t isStationary = 1;
int8_t KF_ZWMIN_KSOT___PF_slot0 = 0x00;
int8_t KF_ZWMIN_KSOT___PF_slot1 = 0x00;
int8_t KF_ZWMIN_KSOT___PF_slot2 = 0x00;
int8_t KF_ZWMIN_KSOT___PF_slot3 = 0x00;
int8_t KF_ZWMIN_KSOT___PF_slot4 = 0x00;
int8_t KF_ZWMIN_KSOT___PF_slot5 = 0x00;
int16_t KL_RF_MAX_UESP___FF_slot1 = 0x1384;
int16_t KL_RF_MAX_UESP___FF_slot2 = 0x0000;
int16_t KL_RF_MAX_UESP___FF_slot3 = 0xc3c3;
int16_t KL_RF_MAX_UESP___FF_slot4 = 0xFFFF;
int16_t KL_RF_MAX_UESP___PF_slot1 = 0x1384;
int16_t KL_RF_MAX_UESP___PF_slot2 = 0x2168;
int16_t KL_RF_MAX_UESP___PF_slot3 = 0x8947;
int16_t KL_RF_MAX_UESP___PF_slot4 = 0x9283;
uint8_t SwitchableMaps_slot1_PF_ValidData = 0;
uint8_t SwitchableMaps_slot2_PF_ValidData = 0;
uint8_t SwitchableMaps_slot3_PF_ValidData = 0;
uint8_t SwitchableMaps_slot4_PF_ValidData = 0;
uint8_t SwitchableMaps_slot1_FF_ValidData = 0;
uint8_t SwitchableMaps_slot2_FF_ValidData = 0;
uint8_t SwitchableMaps_slot3_FF_ValidData = 0;
uint8_t SwitchableMaps_slot4_FF_ValidData = 0;
uint8_t Burbles_slot1_PF_ValidData = 0;
uint8_t Burbles_slot2_PF_ValidData = 0;
uint8_t Burbles_slot3_PF_ValidData = 0;
uint8_t Burbles_slot4_PF_ValidData = 0;
uint8_t Burbles_slot5_PF_ValidData = 0;
uint8_t leavingAntilag = 0;
uint8_t leavingAntilag_timer = 0;
uint8_t leavingDownHold = 0;
uint8_t leavingUpHold = 0;
uint8_t LedStatus = 0;
uint16_t MapSave_NVM = 0;
uint8_t max_cooling_condition = 0;
uint8_t MaxCoolingStatus_EEP = 0;
int16_t Mfl_Left = 0;
uint8_t OnOffDuration = 25; // 250ms    
uint8_t ParametersAreInitialized = 0;
uint8_t pressingAllowed = 0;
uint8_t QuickDuration = 10; // 100ms    
uint16_t rangeMax = 0;
uint16_t rangeMin = 0;
uint8_t RestDuration = 100; // 1000ms    
uint8_t RootMenu = 0;
uint8_t shutdownReason = 0;
uint16_t SlowBlinks = 6969;
uint8_t SwitchableMapSetting_EEP = 0;
uint16_t TargetNumberOfBlinks = 0; // 0x01 == Sensor fault 
uint16_t temp_Value = 0;
uint32_t TempSaveBuffer_SA = 0;
uint16_t timeElapsedSinceLaunch = 0;
uint16_t timer_value_16 = 0;
uint8_t Total_CS_EEP = 0;
uint8_t turnSignal = 0;
uint8_t upPressed = 0;
uint8_t ValetExceededLimits = 0;
uint8_t ValetExceededLimits_EEP = 0;
uint8_t ValetMaxPedal = 50;
int16_t ValetMaxRpm = 4000;
uint16_t ValetMaxSpeed = 50;
uint8_t ValetEnabled = 0;
uint8_t ValetEnabled_EEP = 0;
uint8_t VINcount = 33;
uint16_t CcmEnterMenuDelay = 100;
uint16_t CcmTimeout = 1500;
uint8_t Wdbi_RequestedChange = 0;

// Arduino Functions:
void AddToDebugBuffer(String debugInfo, bool overrideFullFlag = false);
void TestingBitwiseOperations();
void GetUserInput();
void SetLcdRpm();
void SetLedLight();
void PrintDebug();
void DisplayMgflasherBitmap();
void CheckValetPedal();
void CallEcuFunction_GaugeDisplay_Control();
void CallEcuFunction_DTC_Light_Control();
void CallEcuFunction_Mode_Selection();

// ECU Functions:
void GaugeDisplay_Control();
void DTC_Light_Control();
void Mode_Selection();

// Function Timers:
unsigned long PrintDebugTimer_lastMillis;
unsigned long IgnitionOnTimer_lastMillis;
unsigned long GaugeFunctionTimer_lastMillis;
unsigned long GetUserInputTimer_lastMillis;
unsigned long DTC_Light_ControlTimer_lastMillis;
unsigned long Mode_SelectionTimer_lastMillis;

unsigned long LedOnTimer_lastMillis;
unsigned long LedOnDuration;

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

	pinMode(DTC_LEDPin, OUTPUT);

	pinMode(IgnitionPin, INPUT);

	pinMode(BC_ButtonPin, INPUT);
	pinMode(RES_ButtonPin, INPUT);
	pinMode(LongDown_ButtonPin, INPUT);
	pinMode(ShortDown_ButtonPin, INPUT);
	pinMode(ShortUp_ButtonPin, INPUT);
	pinMode(LongUp_ButtonPin, INPUT);

	String welcomeMessage = "Hello, MG Flasher!";
	if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3D)) { // Address 0x3D for 128x64
		Serial.println(F("SSD1306 allocation failed"));
		for (;;); // Don't proceed, loop forever
	}

	lcd.init();
	lcd.backlight();
	lcd.setCursor(0, 0);
	lcd.print(welcomeMessage);
	Serial.println(welcomeMessage);
	DisplayMgflasherBitmap();
	delay(1500);
	display.clearDisplay();
	display.display();
}

void loop() {
	// ECU Functions:
	CallEcuFunction_GaugeDisplay_Control();
	CallEcuFunction_DTC_Light_Control();
	CallEcuFunction_Mode_Selection();

	// Arduino Functions:
	GetUserInput();
	SetLcdRpm();
	SetLedLight();
	CheckValetPedal();
	PrintDebug();
}


// ============================ 
// Arduino Funtions:
// ============================ 
void TestingBitwiseOperations()
{
	MapSave_NVM = 4;								// 3 bits
	custom_burble_status = 5;						// 3 bits
	ExhaustFlapValue = 2;							// 2 bits
	FlexFuelEthanolContentOverrideFlag = 1;			// 1 bit
	FlexFuelEthanolContentOverrideValue = 75;		// 7 bit
	AntilagCounter = 65535;							// 16 bits
	antilagRPMSetpoint = 7000;						// 10 bits
	max_cooling_condition = 1;						// 1 bit
	ValetEnabled = 1;								// 1 bit
	ValetExceededLimits = 1;						// 1 bit

	// 1 byte
	// MapSave_NVM = 4;								// 3 bits
	// custom_burble_status = 5;					// 3 bits
	// ExhaustFlapValue = 2;						// 2 bits
	
	// 1 byte
	// FlexFuelEthanolContentOverrideFlag = 1;		// 1 bit
	// FlexFuelEthanolContentOverrideValue = 75;	// 7 bit
	 
	// 1 byte
	// AntilagCounter = 65535; [LOW BYTE]			// 8 bits

	// 1 byte
	// AntilagCounter = 65535; [HIGH BYTE]			// 8 bits

	// 1 byte
	// antilagRPMSetpoint = 7000; [LOW BYTE]		// 8 bits

	// 1 byte
	// antilagRPMSetpoint = 7000; [HIGH BYTE]		// 2 bits
	// max_cooling_condition = 1;					// 1 bit	
	// ValetEnabled = 1;							// 1 bit
	// ValetExceededLimits = 1;						// 1 bit
	// 3 bits free!
		
	EepromCustomCodeByte0 // Switchable Map Slot + Burble Slot + Exhaust Flap Status
		= ((MapSave_NVM & 0b00000111) << 0)
		+ ((custom_burble_status & 0b00000111) << 3)
		+ ((ExhaustFlapValue & 0b00000011) << 6);

	EepromCustomCodeByte1 // Ethanol Override Status + Ethanol Override Value
		= ((FlexFuelEthanolContentOverrideFlag & 0b00000001) << 0)
		+ ((FlexFuelEthanolContentOverrideValue & 0b01111111) << 1);

	EepromCustomCodeByte2 // Antilag Usage Count (Low byte)
		= AntilagCounter & 0xFF;

	EepromCustomCodeByte3 // Antilag Usage Count (High byte)
		= (AntilagCounter & 0xFF00) >> 8;

	EepromCustomCodeByte4 // Antilag Standing RPM Setpoint (Low byte)
		= (antilagRPMSetpoint / 10) & 0xFF;

	EepromCustomCodeByte5 // Antilag Standing RPM Setpoint (High byte) + Max Cooling Status + Valet Mode Status + Valet Mode Exceeded Limit Status
		= (((((antilagRPMSetpoint / 10) & 0xFF00) >> 8) & 0b00000011) << 0)
		+ ((max_cooling_condition & 0b00000001) << 2)
		+ ((ValetEnabled & 0b00000001) << 3)
		+ ((ValetExceededLimits & 0b00000001) << 4);

	EepromCustomCodeByte6 = 0b00000000; // Free space

	AddToDebugBuffer("\tEEP0: 0b" + String(EepromCustomCodeByte0, BIN) + ",\n");
	AddToDebugBuffer("\tEEP1: 0b" + String(EepromCustomCodeByte1, BIN) + ",\n");
	AddToDebugBuffer("\tEEP2: 0b" + String(EepromCustomCodeByte2, BIN) + ",\n");
	AddToDebugBuffer("\tEEP3: 0b" + String(EepromCustomCodeByte3, BIN) + ",\n");
	AddToDebugBuffer("\tEEP4: 0b" + String(EepromCustomCodeByte4, BIN) + ",\n");
	AddToDebugBuffer("\tEEP5: 0b" + String(EepromCustomCodeByte5, BIN) + ",\n");
	AddToDebugBuffer("\tEEP6: 0b" + String(EepromCustomCodeByte6, BIN) + ",\n");

	MapSave_NVM = (EepromCustomCodeByte0 & 0b00000111) >> 0;
	custom_burble_status = (EepromCustomCodeByte0 & 0b00111000) >> 3;
	ExhaustFlapValue = (EepromCustomCodeByte0 & 0b11000000) >> 6;
	FlexFuelEthanolContentOverrideFlag = (EepromCustomCodeByte1 & 0b00000001) >> 0;
	FlexFuelEthanolContentOverrideValue = (EepromCustomCodeByte1 & 0b11111110) >> 1;
	AntilagCounter = EepromCustomCodeByte2 + (EepromCustomCodeByte3 << 8);
	antilagRPMSetpoint = (EepromCustomCodeByte4 + ((EepromCustomCodeByte5 & 0b00000011) << 8)) * 10;
	max_cooling_condition = (EepromCustomCodeByte5 & 0b00000100) >> 2;
	ValetEnabled = (EepromCustomCodeByte5 & 0b00001000) >> 3;
	ValetExceededLimits = (EepromCustomCodeByte5 & 0b00010000) >> 4;
		
	AddToDebugBuffer("\t MapSave_NVM: " + String(MapSave_NVM) + ",\n");
	AddToDebugBuffer("\t custom_burble_status: " + String(custom_burble_status) + ",\n");
	AddToDebugBuffer("\t ExhaustFlapValue: " + String(ExhaustFlapValue) + ",\n");
	AddToDebugBuffer("\t FlexFuelEthanolContentOverrideFlag: " + String(FlexFuelEthanolContentOverrideFlag) + ",\n");
	AddToDebugBuffer("\t FlexFuelEthanolContentOverrideValue: " + String(FlexFuelEthanolContentOverrideValue) + ",\n");
	AddToDebugBuffer("\t AntilagCounter: " + String(AntilagCounter) + ",\n");
	AddToDebugBuffer("\t antilagRPMSetpoint: " + String(antilagRPMSetpoint) + ",\n");
	AddToDebugBuffer("\t max_cooling_condition: " + String(max_cooling_condition) + ",\n");
	AddToDebugBuffer("\t ValetEnabled: " + String(ValetEnabled) + ",\n");
	AddToDebugBuffer("\t ValetExceededLimits: " + String(ValetExceededLimits) + ",\n");
}

void AddToDebugBuffer(String debugInfo, bool overrideFullFlag)
{
	if (!DebugBufferFull || overrideFullFlag)
	{
		DebugBuffer += debugInfo;
	}
}

void PrintDebug()
{
	if (millis() - PrintDebugTimer_lastMillis < 500UL)
	{
		if (DebugBuffer.length() > 0)
		{
			DebugBufferFull = true;
		}
		return;
	}
	PrintDebugTimer_lastMillis = millis();

	Serial.println(DebugBuffer);
	DebugBuffer = "";
	DebugBufferFull = false;
}

void GetUserInput()
{
	if (millis() - GetUserInputTimer_lastMillis < 50UL)
	{
		return;
	}
	GetUserInputTimer_lastMillis = millis();

	AddToDebugBuffer("Input Values:\n");

	// DTC On time
	{
		//AddToDebugBuffer("\tLED on time: " + String(LedOnDuration) + " ms,\n");
	}

	// Cruise Control
	{
		B_ccResume = digitalRead(RES_ButtonPin);
		B_ccDownLong = digitalRead(LongDown_ButtonPin);
		B_ccDownShort = digitalRead(ShortDown_ButtonPin);
		B_ccUpShort = digitalRead(ShortUp_ButtonPin);
		B_ccUpLong = digitalRead(LongUp_ButtonPin);

		int resButtonValue = B_ccResume ? 0b1111111111111111 & 0x1000 : 0;
		int downLongButtonValue = B_ccDownLong ? (0b1111111111111111 & 0x2) | (0b1111111111111111 & 0x4) : 0;
		int downShortButtonValue = B_ccDownShort ? (0b1111111111111111 & 0x1) | (0b1111111111111111 & 0x4) : 0;
		int upShortButtonValue = B_ccUpShort ? 0b1111111111111111 & 0x1 : 0;
		int upLongButtonValue = B_ccUpLong ? 0b1111111111111111 & 0x2 : 0;
		Mfl_Left = resButtonValue
			+ downLongButtonValue
			+ downShortButtonValue
			+ upShortButtonValue
			+ upLongButtonValue;

		AddToDebugBuffer("\tCruise control: 0x" + String(Mfl_Left, HEX) + ",\n");
	}

	// Turn Signal
	{
		B_ts_BC = digitalRead(BC_ButtonPin);
		int bcButtonValue = B_ts_BC ? 0b1111111111111111 & 0x40 : 0;
		turnSignal = bcButtonValue;

		AddToDebugBuffer("\tTurn signal: 0x" + String(turnSignal, HEX) + ",\n");
	}

	// Ignition
	{
		IgnitionOn = digitalRead(IgnitionPin);
		if (IgnitionOn)
		{
			tnst_w_msg = millis() - IgnitionOnTimer_lastMillis;
			RootMenu = 1;
		}
		else
		{
			tnst_w_msg = 0;
			IgnitionOnTimer_lastMillis = millis();

			LedStatus = true;
			Com_Tx_DispRpmEngCrsh = 65535;

			RootMenu = 0;
			timer_value_16 = 0;
			pressingAllowed = 0;
			currentGaugePosition = 0;
			cancelationTimer = 0;
			cancelationTimerisSet = 0;
			flag_burbleDTC = 0;
			flag_AntilagDTC = 0;
			flag_mapswitchDTC = 0;
			flag_maxCoolingDTC = 0;
			flag_SelectAntilagDTC = 0;
			flag_ethanolContentDisplayDTC = 0;
			flag_EthanolContentOverrideSelectionsDTC = 0;
			flag_EthanolContentOverrideToggleDTC = 0;
			flag_ExhaustFlapSelectionsDTC = 0;
			flag_ValetSelectionsDTC = 0;
			flag_DeveloperSelectionsDTC = 0;
			flag_rootMenuDTC = 0;
			flag_subMenuDTC = 0;
		}

		AddToDebugBuffer("\tEngine ON time: " + String(tnst_w_msg) + " ms,\n");
	}

	// Ethanol Sensor
	{
		int arduinoEthanolContentValue = analogRead(EthanolSensorPin);
		int arduinoFuelTemperatureValue = analogRead(FuelTemperatureSensorPin);
		FlexFuelSensorStatusRawValue = 0;

		FlexFuelSensorEthanolRawValue = map(arduinoEthanolContentValue, 0, 1023, 0, 255); // E % = 8 bit
		FlexFuelSensorTemperatureRawValue = map(arduinoFuelTemperatureValue, 0, 1023, 0, 255); // Fuel Temperature (C) = 8 bit - 40

		// Place into ring buffer (directly after reading from canbus)
		if (CW_SensorEthanol)
		{
			if (FlexFuelBufferIndex >= 7)
			{
				FlexFuelBufferIndex = 0;
			}

			uint8_t volatile* _flexFuelBufferEthanolContentPointer = (uint8_t*)FlexFuelBufferEthanolContent;
			uint8_t volatile* _flexFuelBufferFuelTemperaturePointer = (uint8_t*)FlexFuelBufferFuelTemperature;
			_flexFuelBufferEthanolContentPointer += FlexFuelBufferIndex;
			_flexFuelBufferFuelTemperaturePointer += FlexFuelBufferIndex;
			if (FlexFuelSensorStatusRawValue == 0) // 0x00 == Sensor OK, 0x01 == Sensor fault
			{
				*_flexFuelBufferEthanolContentPointer = FlexFuelSensorEthanolRawValue;
				*_flexFuelBufferFuelTemperaturePointer = FlexFuelSensorTemperatureRawValue;
			}
			else
			{
				*_flexFuelBufferEthanolContentPointer = 0;
				*_flexFuelBufferFuelTemperaturePointer = 0;
			}
			FlexFuelBufferIndex += 1;
		}

		AddToDebugBuffer("\tEthanol Content: " + String(FlexFuelEthanolContentValue) + " %,\n");
		AddToDebugBuffer("\t\tSorted Ethanol Content: [ "
			+ String(FlexFuelBufferEthanolContent[0]) + ", "
			+ String(FlexFuelBufferEthanolContent[1]) + ", "
			+ String(FlexFuelBufferEthanolContent[2]) + ", "
			+ String(FlexFuelBufferEthanolContent[3]) + ", "
			+ String(FlexFuelBufferEthanolContent[4]) + ", "
			+ String(FlexFuelBufferEthanolContent[5]) + ", "
			+ String(FlexFuelBufferEthanolContent[6]) + " ],\n");

		AddToDebugBuffer("\tFuel Temp: " + String(FlexFuelTemperatureValue) + " 'C,\n");
		AddToDebugBuffer("\t\tSorted Fuel Temp: [ "
			+ String(FlexFuelBufferFuelTemperature[0]) + ", "
			+ String(FlexFuelBufferFuelTemperature[1]) + ", "
			+ String(FlexFuelBufferFuelTemperature[2]) + ", "
			+ String(FlexFuelBufferFuelTemperature[3]) + ", "
			+ String(FlexFuelBufferFuelTemperature[4]) + ", "
			+ String(FlexFuelBufferFuelTemperature[5]) + ", "
			+ String(FlexFuelBufferFuelTemperature[6]) + " ],\n");
	}

	// Throttle Pedal
	{
		int arduinoPedalThrottleValue = analogRead(PedalThrottleSensorPin);
		MoFAPP_r = map(arduinoPedalThrottleValue, 0, 1023, 0, 100);
	}

	// Slot Qualifier
	{
		AddToDebugBuffer("\tMapSave_NVM: " + String(MapSave_NVM) + ",\n");
		AddToDebugBuffer("\tPF Slot Values, S1: 0x" + String(KL_RF_MAX_UESP___PF_slot1, HEX) + ", S2: 0x" + String(KL_RF_MAX_UESP___PF_slot2, HEX) + ", S3: 0x" + String(KL_RF_MAX_UESP___PF_slot3, HEX) + ", S4: 0x" + String(KL_RF_MAX_UESP___PF_slot4, HEX) + ",\n");
		AddToDebugBuffer("\tFF Slot Values, S1: 0x" + String(KL_RF_MAX_UESP___FF_slot1, HEX) + ", S2: 0x" + String(KL_RF_MAX_UESP___FF_slot2, HEX) + ", S3: 0x" + String(KL_RF_MAX_UESP___FF_slot3, HEX) + ", S4: 0x" + String(KL_RF_MAX_UESP___FF_slot4, HEX) + ",\n");
	}

	// CCM accessibility
	{
		//AddToDebugBuffer("\tflag_ExhaustFlapSelectionsDTC: " + String(flag_ExhaustFlapSelectionsDTC) + ",\n");
		//AddToDebugBuffer("\ttemp_Value: " + String(temp_Value) + ",\n");

		//AddToDebugBuffer("\tpressingAllowed: " + String(pressingAllowed) + ",\n");
		//AddToDebugBuffer("\tflag_rootMenuDTC: " + String(flag_rootMenuDTC) + ",\n");
		//AddToDebugBuffer("\tflag_subMenuDTC: " + String(flag_subMenuDTC) + ",\n");
	}

	//TestingBitwiseOperations();
}

void SetLcdRpm()
{
	if (Com_Tx_DispRpmEngCrsh_Old != Com_Tx_DispRpmEngCrsh
		|| IgnitionOn != IgnitionOn_old
		|| flag_rootMenuDTC != flag_rootMenuDTC_old
		|| flag_subMenuDTC != flag_subMenuDTC_old
		|| flag_mapswitchDTC != flag_mapswitchDTC_old
		|| flag_maxCoolingDTC != flag_maxCoolingDTC_old
		|| flag_burbleDTC != flag_burbleDTC_old
		|| flag_AntilagDTC != flag_AntilagDTC_old
		|| flag_SelectAntilagDTC != flag_SelectAntilagDTC_old
		|| flag_ethanolContentDisplayDTC != flag_ethanolContentDisplayDTC_old
		|| flag_EthanolContentOverrideSelectionsDTC != flag_EthanolContentOverrideSelectionsDTC_old
		|| flag_EthanolContentOverrideToggleDTC != flag_EthanolContentOverrideToggleDTC_old
		|| flag_ExhaustFlapSelectionsDTC != flag_ExhaustFlapSelectionsDTC_old
		|| flag_ValetSelectionsDTC != flag_ValetSelectionsDTC_old
		|| flag_DeveloperSelectionsDTC != flag_DeveloperSelectionsDTC_old
		|| ValetExceededLimits != ValetExceededLimits_old
		)
	{
		lcd.clear();
		lcd.setCursor(0, 0);
		lcd.print("RPM: ");

		if (Com_Tx_DispRpmEngCrsh == 65535)
		{
			lcd.print("ECU Controlled");

			if (!IgnitionOn)
			{
				lcd.setCursor(0, 3);
				lcd.print("Engine OFF!");
			}
		}
		else
		{
			lcd.print(Com_Tx_DispRpmEngCrsh);
		}

		lcd.setCursor(0, 2);
		if (flag_rootMenuDTC)
		{
			lcd.print("Root Menu");
		}
		else if (flag_subMenuDTC)
		{
			lcd.print("Sub Menu");
		}

		lcd.setCursor(0, 3);
		if (flag_mapswitchDTC)
		{
			lcd.print("Map Switch");
		}
		else if (flag_maxCoolingDTC)
		{
			lcd.print("Max Cooling");
		}
		else if (flag_burbleDTC)
		{
			lcd.print("Burble");
		}
		else if (flag_AntilagDTC)
		{
			lcd.print("Antilag");
		}
		else if (flag_SelectAntilagDTC)
		{
			lcd.print("Select Antilag");
		}
		else if (flag_ethanolContentDisplayDTC)
		{
			lcd.print("Ethanol %");
		}
		else if (flag_EthanolContentOverrideSelectionsDTC)
		{
			lcd.print("Ethanol % O/R");
		}
		else if (flag_EthanolContentOverrideToggleDTC)
		{
			lcd.print("Ethanol O/R Toggle");
		}
		else if (flag_ExhaustFlapSelectionsDTC)
		{
			lcd.print("Exhaust Flap");
		}
		else if (flag_ValetSelectionsDTC)
		{
			lcd.print("Valet Mode");
		}
		else if (flag_DeveloperSelectionsDTC)
		{
			lcd.print("Developer");
		}
		else if (ValetExceededLimits)
		{
			lcd.print("Valet Exceeded Limit");
		}

		Com_Tx_DispRpmEngCrsh_Old = Com_Tx_DispRpmEngCrsh;
		IgnitionOn_old = IgnitionOn;
		flag_rootMenuDTC_old = flag_rootMenuDTC;
		flag_subMenuDTC_old = flag_subMenuDTC;
		flag_mapswitchDTC_old = flag_mapswitchDTC;
		flag_maxCoolingDTC_old = flag_maxCoolingDTC;
		flag_burbleDTC_old = flag_burbleDTC;
		flag_AntilagDTC_old = flag_AntilagDTC;
		flag_SelectAntilagDTC_old = flag_SelectAntilagDTC;
		flag_ethanolContentDisplayDTC_old = flag_ethanolContentDisplayDTC;
		flag_EthanolContentOverrideSelectionsDTC_old = flag_EthanolContentOverrideSelectionsDTC;
		flag_EthanolContentOverrideToggleDTC_old = flag_EthanolContentOverrideToggleDTC;
		flag_ExhaustFlapSelectionsDTC_old = flag_ExhaustFlapSelectionsDTC;
		flag_ValetSelectionsDTC_old = flag_ValetSelectionsDTC;
		flag_DeveloperSelectionsDTC_old = flag_DeveloperSelectionsDTC;
		ValetExceededLimits_old = ValetExceededLimits;
	}
}

void SetLedLight()
{
	if (LedStatus_Old != LedStatus)
	{
		digitalWrite(DTC_LEDPin, LedStatus);
		LedStatus_Old = LedStatus;

		if (LedStatus)
		{
			LedOnDuration = millis() - LedOnTimer_lastMillis;
			LedOnTimer_lastMillis = millis();
			DisplayEngineLightBitmap();
		}
		else
		{
			display.clearDisplay();
			display.display();
		}
	}
}

void DisplayMgflasherBitmap()
{
	display.clearDisplay();
	display.drawBitmap(
		(display.width() - BitmapWidth) / 2,
		(display.height() - BitmapHeight) / 2,
		MgflasherBitmap, BitmapWidth, BitmapHeight, 1);
	display.display();
}

void DisplayEngineLightBitmap()
{
	display.clearDisplay();
	display.drawBitmap(
		(display.width() - BitmapWidth) / 2,
		(display.height() - BitmapHeight) / 2,
		EngineLightBitmap, BitmapWidth, BitmapHeight, 1);
	display.display();
}

void CheckValetPedal()
{
	if (ValetEnabled == 1
		&& MoFAPP_r > ValetMaxPedal)
	{
		ValetExceededLimits = 1;
		ValetExceededLimits_EEP = 1;
	}
}

void CallEcuFunction_GaugeDisplay_Control()
{
	if (millis() - GaugeFunctionTimer_lastMillis < 20UL)
	{
		return;
	}
	GaugeFunctionTimer_lastMillis = millis();
	GaugeDisplay_Control();
}

void CallEcuFunction_DTC_Light_Control()
{
	if (millis() - DTC_Light_ControlTimer_lastMillis < 50UL)
	{
		return;
	}
	DTC_Light_ControlTimer_lastMillis = millis();
	DTC_Light_Control();
}

void CallEcuFunction_Mode_Selection()
{
	if (millis() - Mode_SelectionTimer_lastMillis < 10UL)
	{
		return;
	}
	Mode_SelectionTimer_lastMillis = millis();
	Mode_Selection();
}
































































// ============================
// ECU Funtions:
// ============================

#pragma region Selection Module

#ifndef Skip___Mode_Selection
void Mode_Selection(void) //10ms Task//
{
#ifndef ARDUINO
	__asm__("se_nop");
#endif

	GlobalTimerForEngineRunning32Bit10msResolution++;
	if (tnst_w_msg < 1) // Engine is not running
	{
		GlobalTimerForEngineRunning32Bit10msResolution = 0;
	}

	if (ParametersAreInitialized != 1)
	{
		// Initialize parameters
		ParametersAreInitialized = 1;
		RootMenu = 1;
		RestDuration = 100; // 1000ms
		OnOffDuration = 25; // 250ms
		QuickDuration = 10; // 100ms
		CurrentNumberOfBlinks = 0;
		TargetNumberOfBlinks = 0;
		SlowBlinks = 6969;
		FastBlinks = 9696;
		BlinkRest = 0;
		LedStatus = 0;
		flagBC = 0;
		flag_burbleDTC = 0;
		flag_AntilagDTC = 0;
		flag_mapswitchDTC = 0;
		flag_maxCoolingDTC = 0;
		flag_SelectAntilagDTC = 0;
		flag_ethanolContentDisplayDTC = 0;
		flag_EthanolContentOverrideSelectionsDTC = 0;
		flag_EthanolContentOverrideToggleDTC = 0;
		flag_ExhaustFlapSelectionsDTC = 0;
		flag_ValetSelectionsDTC = 0;
		flag_DeveloperSelectionsDTC = 0;
		flag_rootMenuDTC = 0;
		flag_subMenuDTC = 0;
		upPressed = 0;
		temp_Value = 0;
		downPressed = 0;
		leavingUpHold = 0;
		antilagStatus = 0;
		timer_value_16 = 0;
		pressingAllowed = 0;
		leavingDownHold = 0;
		cancelationTimer = 0;
		currentGaugePosition = 0;
		cancelationTimerisSet = 0;
		FlexFuelBlendingEnabled = CW_FlexFuelBlending;
		InvalidSelectionMade = 0;

		// Initialize buffers
		uint8_t index = 0;
		for (index = 0; index < 7; index++)
		{
			uint8_t volatile* _flexFuelBufferEthanolContentPointer = (uint8_t*)FlexFuelBufferEthanolContent;
			uint8_t volatile* _flexFuelBufferFuelTemperaturePointer = (uint8_t*)FlexFuelBufferFuelTemperature;

			_flexFuelBufferEthanolContentPointer += index;
			_flexFuelBufferFuelTemperaturePointer += index;

			*_flexFuelBufferEthanolContentPointer = 0;
			*_flexFuelBufferFuelTemperaturePointer = 0;
		}

#pragma region Map Switch safety
		uint8_t volatile* _KL_RF_MAX_UESP_PF_slot1_Pointer = (uint8_t*)KL_RF_MAX_UESP___PF_slot1;
		SwitchableMaps_slot1_PF_ValidData =
			!(*_KL_RF_MAX_UESP_PF_slot1_Pointer == 0xC3
				|| *_KL_RF_MAX_UESP_PF_slot1_Pointer == 0xFF
				|| *_KL_RF_MAX_UESP_PF_slot1_Pointer == 0x00
				|| *(_KL_RF_MAX_UESP_PF_slot1_Pointer + 1) == 0xC3
				|| *(_KL_RF_MAX_UESP_PF_slot1_Pointer + 1) == 0xFF
				|| *(_KL_RF_MAX_UESP_PF_slot1_Pointer + 1) == 0x00
				|| *(_KL_RF_MAX_UESP_PF_slot1_Pointer + 2) == 0xC3
				|| *(_KL_RF_MAX_UESP_PF_slot1_Pointer + 2) == 0xFF
				|| *(_KL_RF_MAX_UESP_PF_slot1_Pointer + 2) == 0x00
				|| *(_KL_RF_MAX_UESP_PF_slot1_Pointer + 3) == 0xC3
				|| *(_KL_RF_MAX_UESP_PF_slot1_Pointer + 3) == 0xFF
				|| *(_KL_RF_MAX_UESP_PF_slot1_Pointer + 3) == 0x00);

		uint8_t volatile* _KL_RF_MAX_UESP_PF_slot2_Pointer = (uint8_t*)KL_RF_MAX_UESP___PF_slot2;
		SwitchableMaps_slot2_PF_ValidData =
			!(*_KL_RF_MAX_UESP_PF_slot2_Pointer == 0xC3
				|| *_KL_RF_MAX_UESP_PF_slot2_Pointer == 0xFF
				|| *_KL_RF_MAX_UESP_PF_slot2_Pointer == 0x00
				|| *(_KL_RF_MAX_UESP_PF_slot2_Pointer + 1) == 0xC3
				|| *(_KL_RF_MAX_UESP_PF_slot2_Pointer + 1) == 0xFF
				|| *(_KL_RF_MAX_UESP_PF_slot2_Pointer + 1) == 0x00
				|| *(_KL_RF_MAX_UESP_PF_slot2_Pointer + 2) == 0xC3
				|| *(_KL_RF_MAX_UESP_PF_slot2_Pointer + 2) == 0xFF
				|| *(_KL_RF_MAX_UESP_PF_slot2_Pointer + 2) == 0x00
				|| *(_KL_RF_MAX_UESP_PF_slot2_Pointer + 3) == 0xC3
				|| *(_KL_RF_MAX_UESP_PF_slot2_Pointer + 3) == 0xFF
				|| *(_KL_RF_MAX_UESP_PF_slot2_Pointer + 3) == 0x00);

		uint8_t volatile* _KL_RF_MAX_UESP_PF_slot3_Pointer = (uint8_t*)KL_RF_MAX_UESP___PF_slot3;
		SwitchableMaps_slot3_PF_ValidData =
			!(*_KL_RF_MAX_UESP_PF_slot3_Pointer == 0xC3
				|| *_KL_RF_MAX_UESP_PF_slot3_Pointer == 0xFF
				|| *_KL_RF_MAX_UESP_PF_slot3_Pointer == 0x00
				|| *(_KL_RF_MAX_UESP_PF_slot3_Pointer + 1) == 0xC3
				|| *(_KL_RF_MAX_UESP_PF_slot3_Pointer + 1) == 0xFF
				|| *(_KL_RF_MAX_UESP_PF_slot3_Pointer + 1) == 0x00
				|| *(_KL_RF_MAX_UESP_PF_slot3_Pointer + 2) == 0xC3
				|| *(_KL_RF_MAX_UESP_PF_slot3_Pointer + 2) == 0xFF
				|| *(_KL_RF_MAX_UESP_PF_slot3_Pointer + 2) == 0x00
				|| *(_KL_RF_MAX_UESP_PF_slot3_Pointer + 3) == 0xC3
				|| *(_KL_RF_MAX_UESP_PF_slot3_Pointer + 3) == 0xFF
				|| *(_KL_RF_MAX_UESP_PF_slot3_Pointer + 3) == 0x00);

		uint8_t volatile* _KL_RF_MAX_UESP_PF_slot4_Pointer = (uint8_t*)KL_RF_MAX_UESP___PF_slot4;
		SwitchableMaps_slot4_PF_ValidData =
			!(*_KL_RF_MAX_UESP_PF_slot4_Pointer == 0xC3
				|| *_KL_RF_MAX_UESP_PF_slot4_Pointer == 0xFF
				|| *_KL_RF_MAX_UESP_PF_slot4_Pointer == 0x00
				|| *(_KL_RF_MAX_UESP_PF_slot4_Pointer + 1) == 0xC3
				|| *(_KL_RF_MAX_UESP_PF_slot4_Pointer + 1) == 0xFF
				|| *(_KL_RF_MAX_UESP_PF_slot4_Pointer + 1) == 0x00
				|| *(_KL_RF_MAX_UESP_PF_slot4_Pointer + 2) == 0xC3
				|| *(_KL_RF_MAX_UESP_PF_slot4_Pointer + 2) == 0xFF
				|| *(_KL_RF_MAX_UESP_PF_slot4_Pointer + 2) == 0x00
				|| *(_KL_RF_MAX_UESP_PF_slot4_Pointer + 3) == 0xC3
				|| *(_KL_RF_MAX_UESP_PF_slot4_Pointer + 3) == 0xFF
				|| *(_KL_RF_MAX_UESP_PF_slot4_Pointer + 3) == 0x00);

		uint8_t volatile* _KL_RF_MAX_UESP_FF_slot1_Pointer = (uint8_t*)KL_RF_MAX_UESP___FF_slot1;
		SwitchableMaps_slot1_FF_ValidData =
			!(*_KL_RF_MAX_UESP_FF_slot1_Pointer == 0xC3
				|| *_KL_RF_MAX_UESP_FF_slot1_Pointer == 0xFF
				|| *_KL_RF_MAX_UESP_FF_slot1_Pointer == 0x00
				|| *(_KL_RF_MAX_UESP_FF_slot1_Pointer + 1) == 0xC3
				|| *(_KL_RF_MAX_UESP_FF_slot1_Pointer + 1) == 0xFF
				|| *(_KL_RF_MAX_UESP_FF_slot1_Pointer + 1) == 0x00
				|| *(_KL_RF_MAX_UESP_FF_slot1_Pointer + 2) == 0xC3
				|| *(_KL_RF_MAX_UESP_FF_slot1_Pointer + 2) == 0xFF
				|| *(_KL_RF_MAX_UESP_FF_slot1_Pointer + 2) == 0x00
				|| *(_KL_RF_MAX_UESP_FF_slot1_Pointer + 3) == 0xC3
				|| *(_KL_RF_MAX_UESP_FF_slot1_Pointer + 3) == 0xFF
				|| *(_KL_RF_MAX_UESP_FF_slot1_Pointer + 3) == 0x00);

		uint8_t volatile* _KL_RF_MAX_UESP_FF_slot2_Pointer = (uint8_t*)KL_RF_MAX_UESP___FF_slot2;
		SwitchableMaps_slot2_FF_ValidData =
			!(*_KL_RF_MAX_UESP_FF_slot2_Pointer == 0xC3
				|| *_KL_RF_MAX_UESP_FF_slot2_Pointer == 0xFF
				|| *_KL_RF_MAX_UESP_FF_slot2_Pointer == 0x00
				|| *(_KL_RF_MAX_UESP_FF_slot2_Pointer + 1) == 0xC3
				|| *(_KL_RF_MAX_UESP_FF_slot2_Pointer + 1) == 0xFF
				|| *(_KL_RF_MAX_UESP_FF_slot2_Pointer + 1) == 0x00
				|| *(_KL_RF_MAX_UESP_FF_slot2_Pointer + 2) == 0xC3
				|| *(_KL_RF_MAX_UESP_FF_slot2_Pointer + 2) == 0xFF
				|| *(_KL_RF_MAX_UESP_FF_slot2_Pointer + 2) == 0x00
				|| *(_KL_RF_MAX_UESP_FF_slot2_Pointer + 3) == 0xC3
				|| *(_KL_RF_MAX_UESP_FF_slot2_Pointer + 3) == 0xFF
				|| *(_KL_RF_MAX_UESP_FF_slot2_Pointer + 3) == 0x00);

		uint8_t volatile* _KL_RF_MAX_UESP_FF_slot3_Pointer = (uint8_t*)KL_RF_MAX_UESP___FF_slot3;
		SwitchableMaps_slot3_FF_ValidData =
			!(*_KL_RF_MAX_UESP_FF_slot3_Pointer == 0xC3
				|| *_KL_RF_MAX_UESP_FF_slot3_Pointer == 0xFF
				|| *_KL_RF_MAX_UESP_FF_slot3_Pointer == 0x00
				|| *(_KL_RF_MAX_UESP_FF_slot3_Pointer + 1) == 0xC3
				|| *(_KL_RF_MAX_UESP_FF_slot3_Pointer + 1) == 0xFF
				|| *(_KL_RF_MAX_UESP_FF_slot3_Pointer + 1) == 0x00
				|| *(_KL_RF_MAX_UESP_FF_slot3_Pointer + 2) == 0xC3
				|| *(_KL_RF_MAX_UESP_FF_slot3_Pointer + 2) == 0xFF
				|| *(_KL_RF_MAX_UESP_FF_slot3_Pointer + 2) == 0x00
				|| *(_KL_RF_MAX_UESP_FF_slot3_Pointer + 3) == 0xC3
				|| *(_KL_RF_MAX_UESP_FF_slot3_Pointer + 3) == 0xFF
				|| *(_KL_RF_MAX_UESP_FF_slot3_Pointer + 3) == 0x00);

		uint8_t volatile* _KL_RF_MAX_UESP_FF_slot4_Pointer = (uint8_t*)KL_RF_MAX_UESP___FF_slot4;
		SwitchableMaps_slot4_FF_ValidData =
			!(*_KL_RF_MAX_UESP_FF_slot4_Pointer == 0xC3
				|| *_KL_RF_MAX_UESP_FF_slot4_Pointer == 0xFF
				|| *_KL_RF_MAX_UESP_FF_slot4_Pointer == 0x00
				|| *(_KL_RF_MAX_UESP_FF_slot4_Pointer + 1) == 0xC3
				|| *(_KL_RF_MAX_UESP_FF_slot4_Pointer + 1) == 0xFF
				|| *(_KL_RF_MAX_UESP_FF_slot4_Pointer + 1) == 0x00
				|| *(_KL_RF_MAX_UESP_FF_slot4_Pointer + 2) == 0xC3
				|| *(_KL_RF_MAX_UESP_FF_slot4_Pointer + 2) == 0xFF
				|| *(_KL_RF_MAX_UESP_FF_slot4_Pointer + 2) == 0x00
				|| *(_KL_RF_MAX_UESP_FF_slot4_Pointer + 3) == 0xC3
				|| *(_KL_RF_MAX_UESP_FF_slot4_Pointer + 3) == 0xFF
				|| *(_KL_RF_MAX_UESP_FF_slot4_Pointer + 3) == 0x00);

		uint8_t volatile* _KF_ZWMIN_KSOT_PF_slot1_Pointer = (uint8_t*)KF_ZWMIN_KSOT___PF_slot1;
		Burbles_slot1_PF_ValidData =
			!(*_KF_ZWMIN_KSOT_PF_slot1_Pointer == 0xC3
				|| *_KF_ZWMIN_KSOT_PF_slot1_Pointer == 0xFF
				|| *_KF_ZWMIN_KSOT_PF_slot1_Pointer == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot1_Pointer + 1) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot1_Pointer + 1) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot1_Pointer + 1) == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot1_Pointer + 2) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot1_Pointer + 2) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot1_Pointer + 2) == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot1_Pointer + 3) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot1_Pointer + 3) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot1_Pointer + 3) == 0x00);

		uint8_t volatile* _KF_ZWMIN_KSOT_PF_slot2_Pointer = (uint8_t*)KF_ZWMIN_KSOT___PF_slot2;
		Burbles_slot2_PF_ValidData =
			!(*_KF_ZWMIN_KSOT_PF_slot2_Pointer == 0xC3
				|| *_KF_ZWMIN_KSOT_PF_slot2_Pointer == 0xFF
				|| *_KF_ZWMIN_KSOT_PF_slot2_Pointer == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot2_Pointer + 1) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot2_Pointer + 1) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot2_Pointer + 1) == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot2_Pointer + 2) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot2_Pointer + 2) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot2_Pointer + 2) == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot2_Pointer + 3) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot2_Pointer + 3) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot2_Pointer + 3) == 0x00);

		uint8_t volatile* _KF_ZWMIN_KSOT_PF_slot3_Pointer = (uint8_t*)KF_ZWMIN_KSOT___PF_slot3;
		Burbles_slot3_PF_ValidData =
			!(*_KF_ZWMIN_KSOT_PF_slot3_Pointer == 0xC3
				|| *_KF_ZWMIN_KSOT_PF_slot3_Pointer == 0xFF
				|| *_KF_ZWMIN_KSOT_PF_slot3_Pointer == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot3_Pointer + 1) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot3_Pointer + 1) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot3_Pointer + 1) == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot3_Pointer + 2) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot3_Pointer + 2) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot3_Pointer + 2) == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot3_Pointer + 3) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot3_Pointer + 3) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot3_Pointer + 3) == 0x00);

		uint8_t volatile* _KF_ZWMIN_KSOT_PF_slot4_Pointer = (uint8_t*)KF_ZWMIN_KSOT___PF_slot4;
		Burbles_slot4_PF_ValidData =
			!(*_KF_ZWMIN_KSOT_PF_slot4_Pointer == 0xC3
				|| *_KF_ZWMIN_KSOT_PF_slot4_Pointer == 0xFF
				|| *_KF_ZWMIN_KSOT_PF_slot4_Pointer == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot4_Pointer + 1) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot4_Pointer + 1) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot4_Pointer + 1) == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot4_Pointer + 2) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot4_Pointer + 2) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot4_Pointer + 2) == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot4_Pointer + 3) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot4_Pointer + 3) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot4_Pointer + 3) == 0x00);

		uint8_t volatile* _KF_ZWMIN_KSOT_PF_slot5_Pointer = (uint8_t*)KF_ZWMIN_KSOT___PF_slot5;
		Burbles_slot5_PF_ValidData =
			!(*_KF_ZWMIN_KSOT_PF_slot5_Pointer == 0xC3
				|| *_KF_ZWMIN_KSOT_PF_slot5_Pointer == 0xFF
				|| *_KF_ZWMIN_KSOT_PF_slot5_Pointer == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot5_Pointer + 1) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot5_Pointer + 1) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot5_Pointer + 1) == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot5_Pointer + 2) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot5_Pointer + 2) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot5_Pointer + 2) == 0x00
				|| *(_KF_ZWMIN_KSOT_PF_slot5_Pointer + 3) == 0xC3
				|| *(_KF_ZWMIN_KSOT_PF_slot5_Pointer + 3) == 0xFF
				|| *(_KF_ZWMIN_KSOT_PF_slot5_Pointer + 3) == 0x00);
#pragma endregion Map Switch safety
	}

#pragma region FlexFuel Processing Raw Sensor Values
	if (FlexFuelEthanolContentOverrideFlag)
	{
		FlexFuelEthanolContentValue = FlexFuelEthanolContentOverrideValue / 200;
		if (FlexFuelEthanolContentValue > 100)
		{
			FlexFuelEthanolContentValue = 100;
		}
		FlexFuelTemperatureValue = 0;
	}
	else if (CW_SensorEthanol == 1)
	{
		uint8_t innerLoopIndex = 0;
		uint8_t outerLoopIndex = 0;

		// Bubble sort algorithm:
		for (innerLoopIndex = 0; innerLoopIndex < 6; innerLoopIndex++)
		{
			for (outerLoopIndex = 0; outerLoopIndex < 6 - innerLoopIndex; outerLoopIndex++)
			{
				uint8_t volatile* _flexFuelBufferEthanolContentPointer = (uint8_t*)FlexFuelBufferEthanolContent;
				uint8_t volatile* _flexFuelBufferEthanolContentPointerNext = (uint8_t*)FlexFuelBufferEthanolContent;

				uint8_t outerLoopIndexPlus1 = outerLoopIndex + 1;
				_flexFuelBufferEthanolContentPointer += outerLoopIndex;
				_flexFuelBufferEthanolContentPointerNext += outerLoopIndexPlus1;

				FlexFuelBufferEthanolContentPointerValue = *_flexFuelBufferEthanolContentPointer;
				FlexFuelBufferEthanolContentPointerNextValue = *_flexFuelBufferEthanolContentPointerNext;

				if (FlexFuelBufferEthanolContentPointerValue > FlexFuelBufferEthanolContentPointerNextValue)
				{
					uint8_t tempBufferValue = FlexFuelBufferEthanolContentPointerValue;
					FlexFuelBufferEthanolContentPointerValue = FlexFuelBufferEthanolContentPointerNextValue;
					FlexFuelBufferEthanolContentPointerNextValue = tempBufferValue;
				}
			}
		}
		// Get median
		uint8_t volatile* _flexFuelBufferEthanolContentMedianPointer = (uint8_t*)FlexFuelBufferEthanolContent;
		_flexFuelBufferEthanolContentMedianPointer += 4;
		FlexFuelEthanolContentValue = *_flexFuelBufferEthanolContentMedianPointer; // E % = 8 bit
		if (FlexFuelEthanolContentValue > 100)
		{
			FlexFuelEthanolContentValue = 100;
		}

		// Bubble sort algorithm:
		for (innerLoopIndex = 0; innerLoopIndex < 6; innerLoopIndex++)
		{
			for (outerLoopIndex = 0; outerLoopIndex < 6 - innerLoopIndex; outerLoopIndex++)
			{
				uint8_t volatile* _flexFuelBufferFuelTemperaturePointer = (uint8_t*)FlexFuelBufferFuelTemperature;
				uint8_t volatile* _flexFuelBufferFuelTemperaturePointerNext = (uint8_t*)FlexFuelBufferFuelTemperature;

				uint8_t outerLoopIndexPlus1 = outerLoopIndex + 1;
				_flexFuelBufferFuelTemperaturePointer += outerLoopIndex;
				_flexFuelBufferFuelTemperaturePointerNext += outerLoopIndexPlus1;

				FlexFuelBufferFuelTemperaturePointerValue = *_flexFuelBufferFuelTemperaturePointer;
				FlexFuelBufferFuelTemperaturePointerNextValue = *_flexFuelBufferFuelTemperaturePointerNext;

				if (FlexFuelBufferFuelTemperaturePointerValue > FlexFuelBufferFuelTemperaturePointerNextValue)
				{
					uint8_t tempBufferValue = FlexFuelBufferFuelTemperaturePointerValue;
					FlexFuelBufferFuelTemperaturePointerValue = FlexFuelBufferFuelTemperaturePointerNextValue;
					FlexFuelBufferFuelTemperaturePointerNextValue = tempBufferValue;
				}
			}
		}
		// Get median
		uint8_t volatile* _flexFuelBufferFuelTemperatureMedianPointer = (uint8_t*)FlexFuelBufferFuelTemperature;
		_flexFuelBufferFuelTemperatureMedianPointer += 4;
		FlexFuelTemperatureValue = *_flexFuelBufferFuelTemperatureMedianPointer - 40; // Fuel Temperature (C) = 8 bit - 40
	}
	else
	{
		FlexFuelEthanolContentValue = 0;
		FlexFuelTemperatureValue = 0;
	}
#pragma endregion FlexFuel Processing Raw Sensor Values

#pragma region EEPROM Writing
	if (timeElapsedSinceLaunch <= 100) // 100 * 10ms wait until eeprom can be read
	{
		timeElapsedSinceLaunch++;
		goto exitModeSelection;
	}
	eepDelayFinished = 1;

	uint8_t volatile* EepromBuffer_start = (uint8_t*)EepromBuffer_SA;
	uint8_t volatile* TempSaveBuffer_start = (uint8_t*)TempSaveBuffer_SA;

	bufferTemp = 0;
	if (EepromChecked == 1
		&& (T15_stRaw == 0
			|| Wdbi_RequestedChange == 1)
		)
	{
		Wdbi_RequestedChange = 0;

		// Loading data into EEPROM Custom Code Bytes (Check arduino code for EEPROM byte breakdown):
		EepromCustomCodeByte0 // Switchable Map Slot + Burble Slot + Exhaust Flap Status
			= ((MapSave_NVM & 0b00000111) << 0)
			+ ((custom_burble_status & 0b00000111) << 3)
			+ ((ExhaustFlapValue & 0b00000011) << 6);
		EepromCustomCodeByte1 // Ethanol Override Status + Ethanol Override Value
			= ((FlexFuelEthanolContentOverrideFlag & 0b00000001) << 0)
			+ (((FlexFuelEthanolContentOverrideValue / 200) & 0b01111111) << 1);
		EepromCustomCodeByte2 // Antilag Usage Count (Low byte)
			= AntilagCounter & 0xFF;
		EepromCustomCodeByte3 // Antilag Usage Count (High byte)
			= (AntilagCounter & 0xFF00) >> 8;
		EepromCustomCodeByte4 // Antilag Standing RPM Setpoint (Low byte)
			= (antilagRPMSetpoint / 20) & 0xFF;
		EepromCustomCodeByte5 // Antilag Standing RPM Setpoint (High byte) + Max Cooling Status + Valet Mode Status + Valet Mode Exceeded Limit Status
			= (((((antilagRPMSetpoint / 20) & 0xFF00) >> 8) & 0b00000011) << 0)
			+ ((max_cooling_condition & 0b00000001) << 2)
			+ ((ValetEnabled & 0b00000001) << 3)
			+ ((ValetExceededLimits & 0b00000001) << 4);
		EepromCustomCodeByte6 = 0b00000000; // Free space

		// Store the EEPROM Custom Code Bytes if data has changed:
		Total_CS_EEP = 0x45;
		while (bufferTemp < 7)
		{
			if (*TempSaveBuffer_start != *EepromBuffer_start)
			{
				EepromWriteRequested = 1;
			}

			Total_CS_EEP += *TempSaveBuffer_start;
			TempSaveBuffer_start++;
			EepromBuffer_start++;
			bufferTemp++;
		}

		if (*EepromBuffer_start != Total_CS_EEP)
		{
			EepromWriteRequested = 1;
			checksumInvalid = 1;
		}

		if (EepromWriteRequested == 1)
		{
			EepromWriteRequested = 0;

#ifndef ARDUINO
			__asm__("e_li      %r3, 0x52");                                  // EEP Block Number (HiJackBlock)
			__asm__("e_li      %r4, 120");                                   // Offset From Start Of EEP Block (10 bytes at end of this block are used by us)
			__asm__("e_li      %r5, 8");                                    // Number Of Bytes To Write Into EEP Block
			__asm__("e_lis %r6, " TempSaveBuffer_SA"@ha");					 // Loading Our Custom Buffer Start 
			__asm__("e_add16i  %r6, %r6, " TempSaveBuffer_SA"@l");

			__asm__("se_nop");                                               // Replace this data with actual call to Eep_WriteRam
			__asm__("se_nop");                                               // Rreplace this data with actual call to Eep_WriteRam
#endif
		}
	}

	bufferTemp = 0;
	if (EepromChecked != 1)
	{
		EepromChecked = 1;
		checksumValActual = 0x45; // 0x45 + each byte of the custom code eeprom buffer
		checksumValSaved = 0xFF; // set to the last byte of the custom code eeprom buffer
		while (bufferTemp < 8)
		{
			*TempSaveBuffer_start = *EepromBuffer_start;
			if (bufferTemp < 7)
			{
				checksumValActual += *TempSaveBuffer_start;
			}
			else
			{
				checksumValSaved = *TempSaveBuffer_start;
			}
			TempSaveBuffer_start++;
			EepromBuffer_start++;
			bufferTemp++;
		}

		if (checksumValActual == checksumValSaved)
		{
			EepromLoadedCorrectly = 1; // stored eeprom data is valid, use it
		}
		else
		{
			EepromLoadedCorrectly = 2; // stored eeprom data has invalid checksum, do not use it
		}
		bufferTemp = 0;
	}
	EepromWriteRequested = 0;

#pragma endregion EEPROM Writing

#pragma region Validation Checks
	if (tnst_w_msg < 1) // Engine is not running
	{
		goto exitModeSelection;
	}

	if (leavingAntilag == 1) // Keep system monitoring disable for extra time when leaving antilag
	{
		if (leavingAntilag_timer < 10) // 1 second// 5 = .5 seconds
		{
			leavingAntilag_timer++;
		}
		else
		{
			leavingAntilag = 0;
			leavingAntilag_timer = 0;
		}
	}

	if (isStationary == 1
		&& antilagStatus == 69
		&& Com_Rx_VVehCog >= Antilag_RollingSpeedThreshold) // Standing flagged + rolling + antilag active = shutdown
	{
		AntilagSafetyShutdown = 69;
		antilagStatus = 0;
		leavingAntilag = 1;
	}
#pragma endregion Validation Checks

#pragma region Antilag safety shutdown
	if (AntilagSafetyShutdown == 69
		&& MoFAPP_r <= 0)
	{
		AntilagSafetyShutdown = 0;
	}

	if (antilagStatus == 69
		&& MoFAPP_r > Antilag_Pedal
		&& firstCut == 1)
	{
		if (Antilag_timeElapsed < Antilag_MaxDuration) //Time allowable in Antilag Function
		{
			Antilag_timeElapsed++;
		}
		else
		{
			AntilagSafetyShutdown = 69;
			shutdownReason = 0x20;
			Antilag_timeElapsed = 0;
			leavingAntilag = 1;
			antilagStatus = 0;
		}
	}

	if (CW_Antilag == 1
		&& antilagStatus == 69
		&& AntilagSafetyShutdown == 0
		&&
		(
			tans_msg >= Antilag_IatTempMax
			|| Tmot <= Antilag_CoolantTempMin
			|| Tmot >= Antilag_CoolantTempMax
			|| Tabg_vl_xab >= Antilag_EgtTempMax
			|| (Ps > (Antilag_TargetBoostStand + Antilag_OverBoostThreshold) && isStationary)
			|| (Ps > (Antilag_TargetBoostRoll + Antilag_OverBoostThreshold) && !isStationary)
			)
		)
	{
		if (tans_msg >= Antilag_IatTempMax)
		{
			shutdownReason = 0x08;
		}
		if (Tmot <= Antilag_CoolantTempMin)
		{
			shutdownReason = 0x01;
		}
		if (Tmot >= Antilag_CoolantTempMax)
		{
			shutdownReason = 0x02;
		}
		if (Tabg_vl_xab >= Antilag_EgtTempMax)
		{
			shutdownReason = 0x04;
		}

		if (isStationary)
		{
			if (Ps > (Antilag_TargetBoostStand + Antilag_OverBoostThreshold))
			{
				shutdownReason = 0x10;
			}
		}
		else
		{
			if (Ps > (Antilag_TargetBoostRoll + Antilag_OverBoostThreshold))
			{
				shutdownReason = 0x10;
			}
		}

		AntilagSafetyShutdown = 69;
		antilagStatus = 0;
		leavingAntilag = 1;
	}
#pragma endregion Antilag safety shutdown

#pragma region Selection Module
	if (Mfl_Left != 0 || turnSignal != 0) // Steering button input detected
	{
		cancelationTimer = 0; // Reset cancellation timer
	}

	if ((flag_AntilagDTC && !B_ccDownLong) // Antilag DTC light is blinking & FarDownRocker is NOT pressed
		|| (flag_ethanolContentDisplayDTC && !B_ccUpLong) // Ethanol content display DTC light is blinking & FarUpRocker is NOT pressed
		|| (B_ts_BC || B_ccResume) // OR either BC or RES buttons are pressed
		&& pressingAllowed == 1) // AND pressing buttons is allowed
	{
		if (B_ts_BC || B_ccResume) // Either BC or RES buttons are pressed, confirm selection
		{
			if (flag_rootMenuDTC) // Enter sub menu
			{
				if (B_ts_BC) // Set BC button flag for proper recognition later
				{
					flagBC = 1;
				}
				flag_rootMenuDTC = 0;
				pressingAllowed = 0;
				if (temp_Value == 0)
				{
					flag_subMenuDTC = 0;
					goto exitModeSelection;
				}
				else if (temp_Value == 1)
				{
					flag_subMenuDTC = 1;
					flag_mapswitchDTC = 1;
					temp_Value = MapSave_NVM;
					goto exitModeSelection;
				}
				else if (temp_Value == 2)
				{
					flag_subMenuDTC = 1;
					flag_burbleDTC = 1;
					temp_Value = custom_burble_status;
					goto exitModeSelection;
				}
				else if (temp_Value == 3)
				{
					flag_subMenuDTC = 1;
					flag_maxCoolingDTC = 1;
					temp_Value = max_cooling_condition;
					goto exitModeSelection;
				}
				else if (temp_Value == 4
					&& Com_Rx_VVehCog <= Antilag_RollingSpeedThreshold // AND stationary
					)
				{
					flag_subMenuDTC = 1;
					flag_SelectAntilagDTC = 1;
					temp_Value = antilagRPMSetpoint / 2;
					goto exitModeSelection;
				}
				else if (temp_Value == 5)
				{
					flag_subMenuDTC = 1;
					flag_EthanolContentOverrideToggleDTC = 1;
					temp_Value = FlexFuelEthanolContentOverrideFlag;
					goto exitModeSelection;
				}
				else if (temp_Value == 6)
				{
					flag_subMenuDTC = 1;
					flag_ExhaustFlapSelectionsDTC = 1;
					temp_Value = ExhaustFlapValue;
					goto exitModeSelection;
				}
				else if (temp_Value == 7 && isStationary)
				{
					flag_subMenuDTC = 1;
					flag_ValetSelectionsDTC = 1;
					temp_Value = ValetEnabled;
					goto exitModeSelection;
				}
				//else if (temp_Value == 8)
				//{
				//	flag_subMenuDTC = 1;
				//	flag_DeveloperSelectionsDTC = 1;
				//	temp_Value = developerValue; // whats this?
				//  goto exitModeSelection;
				//}
			}
			else if (flag_mapswitchDTC) // Check map switching and switch maps (if allowed)
			{
				// slot qualifier, if slotXSelected & mapDataSlotX is null, default to zero
				if ((temp_Value == 1 && !SwitchableMaps_slot1_PF_ValidData)
					|| (temp_Value == 2 && !SwitchableMaps_slot2_PF_ValidData)
					|| (temp_Value == 3 && !SwitchableMaps_slot3_PF_ValidData)
					|| (temp_Value == 4 && !SwitchableMaps_slot4_PF_ValidData)
					|| (temp_Value == 1 && !SwitchableMaps_slot1_FF_ValidData && FlexFuelBlendingEnabled == 1)
					|| (temp_Value == 2 && !SwitchableMaps_slot2_FF_ValidData && FlexFuelBlendingEnabled == 1)
					|| (temp_Value == 3 && !SwitchableMaps_slot3_FF_ValidData && FlexFuelBlendingEnabled == 1)
					|| (temp_Value == 4 && !SwitchableMaps_slot4_FF_ValidData && FlexFuelBlendingEnabled == 1)
					)
				{
					MapSave_NVM = 0;
					temp_Value = 0;
					FlexFuelBlendingEnabled = 0;
					FlexFuelEthanolContentOverrideFlag = 0;
					InvalidSelectionMade = 1;
				}
				else
				{
					MapSave_NVM = temp_Value;
				}
			}
			else if (flag_burbleDTC) // Set burble aggression map
			{
				// slot qualifier, if slotXSelected & mapDataSlotX is null, default to zero
				if ((temp_Value == 1 && !Burbles_slot1_PF_ValidData)
					|| (temp_Value == 2 && !Burbles_slot2_PF_ValidData)
					|| (temp_Value == 3 && !Burbles_slot3_PF_ValidData)
					|| (temp_Value == 4 && !Burbles_slot4_PF_ValidData)
					|| (temp_Value == 5 && !Burbles_slot5_PF_ValidData)
					)
				{
					custom_burble_status = 0;
					temp_Value = 0;
					InvalidSelectionMade = 1;
				}
				else
				{
					custom_burble_status = temp_Value;
				}
			}
			else if (flag_maxCoolingDTC) // Enable max cooling
			{
				max_cooling_condition = temp_Value;
			}
			else if (flag_SelectAntilagDTC) // Set antilag rpm
			{
				antilagRPMSetpoint = temp_Value * 2;
				antilagRPMSetpointSave = temp_Value * 2;
			}
			else if (flag_EthanolContentOverrideToggleDTC) // 
			{
				flag_rootMenuDTC = 0;
				pressingAllowed = 0;
				flag_EthanolContentOverrideToggleDTC = 0;
				if (temp_Value == 0)
				{
					flag_subMenuDTC = 0;
					FlexFuelEthanolContentOverrideFlag = 0;
					goto exitModeSelection;
				}
				flag_subMenuDTC = 1;
				flag_EthanolContentOverrideSelectionsDTC = 1;
				temp_Value = FlexFuelEthanolContentOverrideValue / 2;
				goto exitModeSelection;
			}
			else if (flag_EthanolContentOverrideSelectionsDTC) // 
			{
				FlexFuelEthanolContentOverrideValue = temp_Value * 2;
				FlexFuelEthanolContentOverrideFlag = 1;
				FlexFuelBlendingEnabled = 1;
			}
			else if (flag_ExhaustFlapSelectionsDTC) // 
			{
				ExhaustFlapValue = temp_Value;
			}
			else if (flag_ValetSelectionsDTC) // 
			{
				ValetEnabled = temp_Value;
				if (temp_Value == 0)
				{
					ValetExceededLimits = 0;
				}
			}
			//else if (flag_DeveloperSelectionsDTC) // 
			//{
			//	developerValue = temp_Value;
			//}
		}

		if (antilagStatus == 69) // Antilag no longer pressed, exit
		{
			leavingAntilag = 1;
		}

		// Reset selections
		RootMenu = 1;
		upPressed = 0;
		temp_Value = 0;
		downPressed = 0;
		leavingUpHold = 0;
		antilagStatus = 0;
		timer_value_16 = 0;
		flag_burbleDTC = 0;
		flag_AntilagDTC = 0;
		pressingAllowed = 0;
		leavingDownHold = 0;
		flag_mapswitchDTC = 0;
		flag_maxCoolingDTC = 0;
		flag_SelectAntilagDTC = 0;
		flag_subMenuDTC = 0;
		flag_ethanolContentDisplayDTC = 0;
		flag_EthanolContentOverrideSelectionsDTC = 0;
		flag_EthanolContentOverrideToggleDTC = 0;
		flag_ExhaustFlapSelectionsDTC = 0;
		flag_ValetSelectionsDTC = 0;
		flag_DeveloperSelectionsDTC = 0;
		currentGaugePosition = 0;
		flag_rootMenuDTC = 0;
	}

	// Entering buttons:

	// CC Menu:
	if (CW_CCMenu == 1 // CC Menu codeword enabled
		&& RootMenu == 1 // Root Menu Allowed
		&& (B_ts_BC || B_ccResume) // AND BC/RES selection buton pressed
		&& !flag_rootMenuDTC // AND CC Menu DTC light NOT blinking (not already selecting CC Menu settings)
		&& pressingAllowed != 1) // AND Pressing buttons is NOT allowed?
	{
		flagBC = 0;

		if (B_ts_BC) // Set BC button flag for proper recognition later
		{
			flagBC = 1;
		}

		if (timer_value_16 < CcmEnterMenuDelay) // Hold for xxx seconds to enter CC menu
		{
			timer_value_16++;
		}
		else
		{ // Set required flags to enter CC menu
			flag_SelectAntilagDTC = 0;
			flag_ethanolContentDisplayDTC = 0;
			flag_EthanolContentOverrideSelectionsDTC = 0;
			flag_EthanolContentOverrideToggleDTC = 0;
			flag_ExhaustFlapSelectionsDTC = 0;
			flag_ValetSelectionsDTC = 0;
			flag_DeveloperSelectionsDTC = 0;
			temp_Value = 0;
			flag_mapswitchDTC = 0;
			flag_AntilagDTC = 0;
			flag_maxCoolingDTC = 0;
			flag_burbleDTC = 0;
			timer_value_16 = 0;
			flag_rootMenuDTC = 1;
			cancelationTimerisSet = 1;
		}
	}

	// Display flex fuel ethanol content:
	else if (CW_FlexFuel == 1 // Flex fuel enabled
		&& B_ccUpLong // AND ethanol content display buton pressed
		&& !flag_ethanolContentDisplayDTC // AND ethanol content display DTC light NOT blinking
		&& pressingAllowed != 1) // AND Pressing buttons is NOT allowed?
	{
		if (timer_value_16 < FlexFuelEthanolDisplayActivationDelay) // Hold for xxx seconds to display ethanol content
		{
			timer_value_16++;
		}
		else
		{ // Set required flags to display ethanol content
			flag_SelectAntilagDTC = 0;
			flag_ethanolContentDisplayDTC = 1;
			flag_EthanolContentOverrideSelectionsDTC = 0;
			flag_EthanolContentOverrideToggleDTC = 0;
			flag_ExhaustFlapSelectionsDTC = 0;
			flag_ValetSelectionsDTC = 0;
			flag_DeveloperSelectionsDTC = 0;
			cancelationTimerisSet = 1;
			temp_Value = FlexFuelEthanolContentValue * 100;
			flag_mapswitchDTC = 0;
			flag_AntilagDTC = 0;
			flag_maxCoolingDTC = 0;
			flag_burbleDTC = 0;
			timer_value_16 = 0;
			flag_rootMenuDTC = 0;
		}
	}

	// Enter Antilag
	else if (CW_Antilag == 1 // Antilag codeword enabled
		&& B_ccDownLong // AND Antilag selection buton pressed
		&& Tmot >= Antilag_CoolantTempMin // AND engine is warm enough
		&& AntilagSafetyShutdown == 0 // AND not in Antilag Shutdown
		&& !flag_AntilagDTC // AND Antilag DTC light NOT blinking (not already using Antilag)
		&& pressingAllowed != 1) // AND Pressing buttons is NOT allowed?
	{
		if (Com_Rx_VVehCog >= Antilag_RollingSpeedThreshold // Car is rolling
			&& (Epm_nEng < Antilag_RollRpmMin || Epm_nEng > Antilag_RollRpmMax)) // AND RPM is OUT of target range
		{
			goto endOfAntilag;
		}

		if (timer_value_16 < Antilag_ActivationDelay) // Hold for xxx seconds to enter Antilag
		{
			timer_value_16++;
		}
		else
		{
			if (Com_Rx_VVehCog >= Antilag_RollingSpeedThreshold) // NOT stationary, set rolling RPM
			{
				antilagRPMSetpoint = Epm_nEng;
				flagDynamicAntilagRPM = 1;
			}
			else if (flagDynamicAntilagRPM == 1) // use rolling antilag, use dynamically saved RPM
			{
				antilagRPMSetpoint = antilagRPMSetpointSave;
				flagDynamicAntilagRPM = 0;
			}
			else // stationary, used preset RPM
			{
				antilagRPMSetpoint = antilagRPMSetpointSave;
			}

			if (Com_Rx_VVehCog < Antilag_RollingSpeedThreshold) // check if stationary
			{
				isStationary = 1;
			}
			else
			{
				isStationary = 0;
			}

			// Set required flags to use Antilag
			firstCut = 0;
			Antilag_timeElapsed = 0;
			antilagStatus = 69;
			shutdownReason = 0;
			flag_AntilagDTC = 1;
			flag_mapswitchDTC = 0;
			flag_maxCoolingDTC = 0;
			flag_burbleDTC = 0;
			timer_value_16 = 0;
			flag_SelectAntilagDTC = 0;
			flag_ethanolContentDisplayDTC = 0;
			flag_EthanolContentOverrideSelectionsDTC = 0;
			flag_EthanolContentOverrideToggleDTC = 0;
			flag_ExhaustFlapSelectionsDTC = 0;
			flag_ValetSelectionsDTC = 0;
			flag_DeveloperSelectionsDTC = 0;
			cancelationTimerisSet = 1;
		}
	endOfAntilag:
#ifndef ARDUINO
		__asm__("se_nop");
#else
		int ADD_ASM_CODE = 0;
#endif
	}
	else
	{
		timer_value_16 = 0;
	}

	if (cancelationTimerisSet == 1)
	{
		if (cancelationTimer < CcmTimeout) // xxx seconds before the ccm times out//
		{
			cancelationTimer++;
		}
		else
		{
			upPressed = 0;
			temp_Value = 0;
			downPressed = 0;
			leavingUpHold = 0;
			antilagStatus = 0;
			timer_value_16 = 0;
			pressingAllowed = 0;
			leavingDownHold = 0;
			cancelationTimer = 0;
			currentGaugePosition = 0;
			cancelationTimerisSet = 0;
			flag_burbleDTC = 0;
			flag_AntilagDTC = 0;
			flag_mapswitchDTC = 0;
			flag_maxCoolingDTC = 0;
			flag_SelectAntilagDTC = 0;
			flag_ethanolContentDisplayDTC = 0;
			flag_EthanolContentOverrideSelectionsDTC = 0;
			flag_EthanolContentOverrideToggleDTC = 0;
			flag_ExhaustFlapSelectionsDTC = 0;
			flag_ValetSelectionsDTC = 0;
			flag_DeveloperSelectionsDTC = 0;
			flag_rootMenuDTC = 0;
			flag_subMenuDTC = 0;
			RootMenu = 1;
		}
	}
#pragma endregion Selection Module

	exitModeSelection :
#ifndef ARDUINO
	__asm__("se_nop");
#else
	int ADD_ASM_CODE = 0;
#endif
}
#endif

#ifndef Skip___DTC_Light_Control
void DTC_Light_Control(void) //50ms Task//
{
#ifndef ARDUINO
	__asm__("se_nop");
#endif

	register uint32_t PHYmil __asm("r0");

	if (tnst_w_msg < 1 // Engine is NOT running
		|| ParametersAreInitialized != 1) // Parameters are not initialized
	{
		goto exitDtcLightControl;
	}

	if (VINcount == 123) // vin check?
	{
#ifndef ARDUINO
		__asm__("e_b 0xFF000000");
#endif
	}

	ControlLed = 0;

	if (flag_mapswitchDTC == 1 // Slow blink
		|| flag_burbleDTC == 1 // Slow blink
		|| flag_maxCoolingDTC == 1 // Slow blink
		|| flag_SelectAntilagDTC == 1 // Slow blink
		|| flag_AntilagDTC == 1 // Fast blink
		|| flag_ethanolContentDisplayDTC == 1 // Slow blink
		|| flag_rootMenuDTC == 1 // Slow blink
		|| flag_subMenuDTC == 1  // Blink xxx times
		|| flag_EthanolContentOverrideSelectionsDTC == 1 // Slow blink
		|| flag_EthanolContentOverrideToggleDTC == 1 // Slow blink
		|| flag_ExhaustFlapSelectionsDTC == 1 // Slow blink
		|| flag_ValetSelectionsDTC == 1 // Slow blink
		|| flag_DeveloperSelectionsDTC == 1 // Slow blink
		|| ValetExceededLimits == 1 // Slow blink
		) // DTC light is requested
	{
		ControlLed = 1;

		// Set blinking frequency depending on selection mode
		if (flag_AntilagDTC)
		{
			TargetNumberOfBlinks = FastBlinks;
		}
		else if (flag_subMenuDTC)
		{
			TargetNumberOfBlinks = currentGaugePosition;
		}
		else
		{
			TargetNumberOfBlinks = SlowBlinks;
		}
	}

	if (!ControlLed)
	{
		CelLastMillis10ms = GlobalTimerForEngineRunning32Bit10msResolution;
		CurrentNumberOfBlinks = 0;
		BlinkRest = 0;
		LedStatus = 0;
		goto exitDtcLightControl;
	}

	if (TargetNumberOfBlinks == SlowBlinks)
	{
		BlinkDuration = RestDuration;
	}
	else if (TargetNumberOfBlinks == FastBlinks)
	{
		BlinkDuration = QuickDuration;
	}
	else
	{
		BlinkDuration = OnOffDuration;
	}

	if (!BlinkRest && CurrentNumberOfBlinks < TargetNumberOfBlinks)
	{
		if (GlobalTimerForEngineRunning32Bit10msResolution - CelLastMillis10ms < BlinkDuration)
		{
			LedStatus = 1; //LED ON
		}
		else if (GlobalTimerForEngineRunning32Bit10msResolution - CelLastMillis10ms < BlinkDuration * 2)
		{
			LedStatus = 0; //LED OFF
		}
		else
		{
			CurrentNumberOfBlinks++;
			CelLastMillis10ms = GlobalTimerForEngineRunning32Bit10msResolution;
		}

		BlinkRest = 0;
	}
	else
	{
		CurrentNumberOfBlinks = 0;
		BlinkRest = 1;
	}

	if (BlinkRest)
	{
		if (GlobalTimerForEngineRunning32Bit10msResolution - CelLastMillis10ms < RestDuration)
		{
			LedStatus = 0; //LED OFF
		}
		else
		{
			CelLastMillis10ms = GlobalTimerForEngineRunning32Bit10msResolution;
			BlinkRest = 0;
		}
	}

	if (LedStatus == 1)
	{
		PHYmil = 0xFF; //LED ON
	}
	else
	{
		PHYmil = 0x00; //LED OFF
	}

exitDtcLightControl:
#ifndef ARDUINO
	__asm__("e_lis %r7, "MILLmp_stMIL"@ha");
	__asm__("e_stb %r0, "MILLmp_stMIL"@l(%r7)");

	__asm__("se_nop");
#else
	int ADD_ASM_CODE = 0;
#endif
}
#endif

#ifndef Skip___GaugeDisplay_Control
void GaugeDisplay_Control(void) //20ms Task//
{
#ifndef ARDUINO
	__asm__("se_nop");

	__asm__("e_stwu   %r1, -0x18(%r1)");
	__asm__("se_stw   %r0,  0x4(%r1)");
	__asm__("se_stw   %r5,  0x8(%r1)");
	__asm__("se_stw   %r6,  0xC(%r1)");
	__asm__("se_stw   %r7,  0x10(%r1)");
#endif

	if (tnst_w_msg < 1 // Engine is NOT running
		|| ParametersAreInitialized != 1) // Parameters are not initialized
	{
		goto exitGaugeDisplayControl;
	}

	// Set RPM selection range per mode
	if (flag_rootMenuDTC && !flag_subMenuDTC)
	{
		rangeMax = 3;
		rangeMin = 0;
		if (CW_IsDeveloper == 1)
		{
			rangeMax = 8;
		}
		else if (CW_Valet == 1)
		{
			rangeMax = 7;
		}
		else if (CW_ExhaustFlap == 1)
		{
			rangeMax = 6;
		}
		else if (CW_FlexFuel == 1)
		{
			rangeMax = 5;
		}
		else if (CW_Antilag == 1)
		{
			rangeMax = 4;
		}
	}
	else if (flag_maxCoolingDTC)
	{
		rangeMax = 1;
		rangeMin = 0;
	}
	else if (flag_burbleDTC)
	{
		rangeMax = 5;
		rangeMin = 0;
	}
	else if (flag_mapswitchDTC)
	{
		rangeMax = 4;
		rangeMin = 0;
	}
	else if (flag_SelectAntilagDTC)
	{
		rangeMax = 6000;
		rangeMin = 1500;
	}
	else if (flag_ethanolContentDisplayDTC)
	{
		rangeMax = 7500; // what's the max gauge position? :)
		rangeMin = 0;
		temp_Value = FlexFuelEthanolContentValue * 100;
		if (temp_Value > rangeMax)
		{
			temp_Value = rangeMax;
		}
	}
	else if (flag_EthanolContentOverrideToggleDTC)
	{
		rangeMax = 1;
		rangeMin = 0;
	}
	else if (flag_EthanolContentOverrideSelectionsDTC)
	{
		rangeMax = 7500;
		rangeMin = 0;
	}
	else if (flag_ExhaustFlapSelectionsDTC)
	{
		rangeMax = 2; // 0 = oem function, 1 = closed flap, 2 = open flap
		rangeMin = 0;
	}
	else if (flag_ValetSelectionsDTC)
	{
		rangeMax = 1;
		rangeMin = 0;
	}
	//else if (flag_DeveloperSelectionsDTC)
	//{
	//	rangeMax = 7500;
	//	rangeMin = 0;
	//}
	else if (InvalidSelectionMade == 1)
	{
		temp_Value = 7;
		InvalidSelectionMadeLastMillis10ms = GlobalTimerForEngineRunning32Bit10msResolution;
		InvalidSelectionMade = 0;
	}

	// Set button allowances
	if (leavingDownHold == 1
		|| leavingUpHold == 1
		|| flag_mapswitchDTC == 1
		|| flag_SelectAntilagDTC == 1
		|| flag_rootMenuDTC == 1
		|| flag_subMenuDTC == 1
		|| flag_EthanolContentOverrideSelectionsDTC == 1
		)
	{
		if ((flagBC && turnSignal == 0)
			|| (!flagBC && Mfl_Left == 0))
		{
			pressingAllowed = 1;
			leavingDownHold = 0;
			leavingUpHold = 0;
			flagBC = 0;
		}
	}

	// Set timers and up/down button flags
	if (flag_SelectAntilagDTC == 1
		|| flag_EthanolContentOverrideSelectionsDTC == 1)
	{
		if (pressingAllowed == 1
			&& B_ccUpShort)
		{
			upPressed = 1;
			timer_value_16 = 0;
		}
		else if (pressingAllowed == 1
			&& B_ccDownShort)
		{
			downPressed = 1;
			timer_value_16 = 0;
		}
	}
	else
	{
		if (pressingAllowed == 1
			&& B_ccUpShort_LastStatus != B_ccUpShort
			&& B_ccUpShort)
		{
			upPressed = 1;
			timer_value_16 = 0;
		}

		if (pressingAllowed == 1
			&& B_ccDownShort_LastStatus != B_ccDownShort
			&& B_ccDownShort)
		{
			downPressed = 1;
			timer_value_16 = 0;
		}

		if (pressingAllowed == 1 && flag_ethanolContentDisplayDTC == 1)
		{
			timer_value_16 = 0;
		}
	}

	// Set RPM gauge value
	if ((CW_BOTF == 1 && flag_burbleDTC == 1)
		|| (CW_max_cooling == 1 && flag_maxCoolingDTC == 1)
		|| (CW_OctaneSwitch == 1 && flag_mapswitchDTC == 1)
		|| (CW_Antilag == 1 && flag_SelectAntilagDTC == 1)
		|| (CW_SensorEthanol == 1 && flag_ethanolContentDisplayDTC == 1)
		|| (CW_FlexFuel == 1 && flag_EthanolContentOverrideSelectionsDTC == 1)
		|| (CW_FlexFuel == 1 && flag_EthanolContentOverrideToggleDTC == 1)
		|| (CW_ExhaustFlap == 1 && flag_ExhaustFlapSelectionsDTC == 1)
		|| (CW_Valet == 1 && flag_ValetSelectionsDTC == 1)
		|| (CW_IsDeveloper == 1 && flag_DeveloperSelectionsDTC == 1)
		|| (CW_CCMenu == 1 && flag_rootMenuDTC == 1)
		)
	{
		if (pressingAllowed == 1
			&& flag_ethanolContentDisplayDTC != 1
			)
		{
			if (upPressed == 1)
			{
				if (temp_Value < rangeMax)
				{
					if (flag_SelectAntilagDTC == 1)
					{
						temp_Value += 10;
					}
					else if (flag_EthanolContentOverrideSelectionsDTC == 1)
					{
						temp_Value += 10;
					}
					else
					{
						temp_Value++;
					}
				}
			}
			else if (downPressed == 1)
			{
				if (temp_Value > rangeMin)
				{
					if (flag_SelectAntilagDTC == 1)
					{
						temp_Value -= 10;
					}
					else if (flag_EthanolContentOverrideSelectionsDTC == 1)
					{
						temp_Value -= 10;
					}
					else
					{
						temp_Value--;
					}
				}
			}
			upPressed = 0;
			downPressed = 0;
		}

		if (temp_Value <= rangeMax)
		{
			if (flag_SelectAntilagDTC == 1)
			{
				//     (     (temp_Value   Antilag_RpmDelayUntilCutX) )
				// 5 + (96 * (---------- + -------------------------) )
				//     (     (    10                   20           ) )
				// ----------------------------------------------------
				//                        100
#ifdef ARDUINO
				Com_Tx_DispRpmEngCrsh = temp_Value; // for nicer LCD display
#else
				Com_Tx_DispRpmEngCrsh = ((96 * ((temp_Value / 10) + (Antilag_RpmDelayUntilCutStand / 20))) + 5) / 100; // rescaled for antilag
#endif
			}
			else if (flag_EthanolContentOverrideSelectionsDTC == 1)
			{
				//     (     (temp_Value) )
				// 5 + (96 * (----------) )
				//     (     (    10    ) )
				// ------------------------
				//           100
#ifdef ARDUINO
				Com_Tx_DispRpmEngCrsh = temp_Value; // for nicer LCD display
#else
				Com_Tx_DispRpmEngCrsh = ((96 * (temp_Value / 10)) + 5) / 100; // Requires some finnessinng
#endif
			}
			else if (flag_ethanolContentDisplayDTC == 1)
			{
				//     (     (temp_Value) )
				// 5 + (96 * (----------) )
				//     (     (    10    ) )
				// ------------------------
				//           100
#ifdef ARDUINO
				Com_Tx_DispRpmEngCrsh = temp_Value; // for nicer LCD display
#else
				Com_Tx_DispRpmEngCrsh = ((96 * (temp_Value / 10)) + 5) / 100; // Requires some finnessinng
#endif
			}
			else
			{
#ifdef ARDUINO
				Com_Tx_DispRpmEngCrsh = temp_Value * 1000; // for nicer LCD display
#else
				Com_Tx_DispRpmEngCrsh = (96 * temp_Value) + 5; //To allow for inconsistencies in dash rpm display//
#endif
			}

			if (flag_rootMenuDTC)
			{
				currentGaugePosition = temp_Value;
			}
		}
	}
	else if (GlobalTimerForEngineRunning32Bit10msResolution - InvalidSelectionMadeLastMillis10ms < RestDuration)
	{
#ifdef ARDUINO
		Com_Tx_DispRpmEngCrsh = temp_Value * 1000; // for nicer LCD display
#else
		Com_Tx_DispRpmEngCrsh = (96 * temp_Value) + 5; //To allow for inconsistencies in dash rpm display//
#endif
	}
	else
	{
		// Set normal gauge operation
#ifdef ARDUINO
		Com_Tx_DispRpmEngCrsh = 65535; // ARDUINO ONLY
#endif

#ifdef GxxSoftware

#ifndef ARDUINO
		__asm__("se_lwz   %r7,  0x10(%r1)");
		__asm__("e_lis %r6, "Com_Tx_DispRpmEngCrsh"@ha");
		__asm__("e_sth %r7, "Com_Tx_DispRpmEngCrsh"@l(%r6)");
#endif
#else
#ifndef ARDUINO
		__asm__("e_lis %r7, "Com_Tx_DispRpmEngCrsh"@ha");
		__asm__("e_sth %r0, "Com_Tx_DispRpmEngCrsh"@l(%r7)");
#endif
#endif
	}
	B_ccDownShort_LastStatus = B_ccDownShort;
	B_ccUpShort_LastStatus = B_ccUpShort;

exitGaugeDisplayControl:

#ifndef ARDUINO
	__asm__("se_lwz   %r0,  0x4(%r1)");
	__asm__("se_lwz   %r5,  0x8(%r1)");
	__asm__("se_lwz   %r6,  0xC(%r1)");
	__asm__("se_lwz   %r7,  0x10(%r1)");
	__asm__("e_add16i %r1, %r1, 0x18");

	__asm__("se_nop");
#else
	int ADD_ASM_CODE = 0;
#endif
}
#endif

#pragma endregion Selection Module