/*===========================================================================================================
                                              Variables/Setup
=========================================================================================================== */

//Library setup
#include <Wire.h>
//#include <Adafruit_GFX.h> //Screen graphics library
//#include <Adafruit_SSD1306.h> //Screen library
#include <MAX6675.h> //Thermocouple chip library
#include <ezButton.h> //Joystick button library
#include <U8g2lib.h>
/*Screen Setup SSD1306
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
unsigned long lastDisplayUpdate = 0;
const unsigned long DisplayInterval = 100; //Display Refresh Rate
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, -1); */

//Screen Setup ST7567

U8G2_ST7567_HEM6432_F_HW_I2C u8g2(U8G2_R0, /* reset=*/ U8X8_PIN_NONE);
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
unsigned long lastDisplayUpdate = 0;
const unsigned long DisplayInterval = 100; //Display Refresh Rate

//Screen Switching (Multi-screen) Setup
int CurrentScreen = 0; //Always starts on default screen
unsigned long LastNavTime = 0;
const unsigned long NavDelay = 300;

//Screen Brightness Setup
int ScreenBrightness = 5; //Sets initial screen brightness value
unsigned long LastAdjustTime = 0;
const unsigned long AdjustDelay = 200;

//Joystick & Button Setup
#define JOY_X_PIN  A0 // Arduino pin connected to x axis pin
#define JOY_Y_PIN  A1 // Arduino pin connected to y axis pin
#define JOY_B_PIN   22  // Arduino pin connected to button  pin

ezButton button(JOY_B_PIN);

int xValue = 0; // To store value of the X axis
int yValue = 0; // To store value of the Y axis
int bValue = 0; // To store value of the button


//Smoothing Function Setup
const int numReadings = 10; //Smoothing Level, MAXIMUM 35

//EGT Type K Thermocouple Setup
int SO_PIN = 5; //Serial Out Pin
int CS_PIN = 6; //Chip Select Pin
int CK_PIN = 7; //Clock Pin
int ExhaustTotal = 0;
int ExhaustReadIndex = 0;
int ExhaustReadings[numReadings];
MAX6675 thermocouple(CK_PIN, CS_PIN, SO_PIN);

//Fuel Pressure Transducer Setup
#define FuelPressurePin 13
const int PTransMin = 102; //102.4 = 0.5V (Pressure Transducer = 0.5-4.5V Range)
const int PTransMax = 922; //921.6 = 4.5V (Pressure Transducer = 0.5-4.5V Range)
int FuelReadIndex = 0;
int FuelReadings[numReadings];  // the readings from the analog input
int FuelTotal = 0;              // the running total
int FuelPressurePinAverage = 0;            // the average

//Fuel Pressure Alarm Setup
bool FuelAlarmActive = false;
unsigned long FuelLowStartTime = 0;
unsigned long LastFuelFlashTime = 0;
bool ShowFuelText = true;

/*----------------------------------------------------------------------
                             Screen Render Function
------------------------------------------------------------------------*/

void setup() {
Serial.begin(9600);
// initialize all the readings to 0:
  for (int thisFuelReading = 0; thisFuelReading < numReadings; thisFuelReading++) {
    FuelReadings[thisFuelReading] = 0;
  }
  for (int thisExhaustReading = 0; thisExhaustReading < numReadings; thisExhaustReading++) {
    ExhaustReadings[thisExhaustReading] = 0;
  }

button.setDebounceTime(50);
u8g2.begin();
/*
if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) {
 Serial.println(F("SSD1306 allocation failed"));
 for (;;); // Don't proceed, loop forever
}

display.clearDisplay();
display.setTextColor(SSD1306_WHITE);*/
WelcomeScreen();
}

/*----------------------------------------------------------------------
                             Loop Code
------------------------------------------------------------------------*/

void loop() {
unsigned long currentMillis = millis();
float FuelPressureDisplay = FuelPressureRead();
//float BoostPressureDisplay = BoostPressureRead();
float ExhaustTempDisplay = ExhaustTempRead();

//Function Calls
ScreenNavigation();
//AdjustBrightness();

//Fuel Alarm Interrupt
FuelPressureAlarm(FuelPressureDisplay, currentMillis);
if (FuelAlarmActive) return;

//Render/Delay Function
if (currentMillis - lastDisplayUpdate >= DisplayInterval) {
    lastDisplayUpdate = currentMillis;
    RenderCurrentScreen(ExhaustTempDisplay, FuelPressureDisplay);
}
/*
display.fillRect(0,0,128,64,BLACK);
//Display Parameters EGT
display.setCursor(0, 6);
display.setTextSize(2);
display.setTextColor(SSD1306_WHITE);
display.print("EGT:");
display.print(ExhaustTempDisplay,0);
display.print((char)247);
display.print("F");

//Display Parameters Fuel
display.setCursor(0, 44); // Start lower on screen
display.print("FP:");
display.print(FuelPressureDisplay,1);
display.setTextSize(2);
display.print("PSI");
display.drawLine(0, 31, 127, 31, SSD1306_WHITE);
display.display();
}
*/
}

/*----------------------------------------------------------------------
                             Fuel Pressure Sensor Function
------------------------------------------------------------------------*/

float FuelPressureRead() {
  
  const int FuelPTransMax = 300; //Fuel PTrans = 0-30psi, scaled 10x for final resolution

//float FuelPressure = 0.1 * map(analogRead(FuelPressurePin), PTransMin, PTransMax, 0, FuelPTransMax); //MAP function only does integers... Scale range 10x and divide by 10 outside of function to get a decimal place back.

//Smoothing Program

  // subtract the last reading:
  FuelTotal = FuelTotal - FuelReadings[FuelReadIndex];
  // read from the sensor:
  FuelReadings[FuelReadIndex] = analogRead(FuelPressurePin);
  // add the reading to the total:
  FuelTotal = FuelTotal + FuelReadings[FuelReadIndex];
  // advance to the next position in the array:
  FuelReadIndex = FuelReadIndex + 1;

  // if we're at the end of the array...
  if (FuelReadIndex >= numReadings) {
    // ...wrap around to the beginning:
    FuelReadIndex = 0;
  }

  // calculate the average:
  FuelPressurePinAverage = FuelTotal / numReadings;
  return 0.1 * map(FuelPressurePinAverage, PTransMin, PTransMax, 0, FuelPTransMax);
}
  /* send it to the computer as ASCII digits
  Serial.print(FuelPressurePinAverage);
  Serial.print(',');
  Serial.print(analogRead(FuelPressurePin));
  Serial.print(',');
  Serial.print(FuelPressureAverage);
  Serial.print(',');
  Serial.println(FuelPressure);
  delay(1);  // delay in between reads for stability
*/
/*----------------------------------------------------------------------
                             EGT Sensor Function
------------------------------------------------------------------------*/
float ExhaustTempRead(){
//Smoothing Program

  // subtract the last reading:
  ExhaustTotal = ExhaustTotal - ExhaustReadings[ExhaustReadIndex];
  // read from the sensor:
  ExhaustReadings[ExhaustReadIndex] = 500;
  //ExhaustReadings[ExhaustReadIndex] = thermocouple.getTemperature();
  // add the reading to the total:
  ExhaustTotal = ExhaustTotal + ExhaustReadings[ExhaustReadIndex];
  // advance to the next position in the array:
  ExhaustReadIndex = ExhaustReadIndex + 1;

  // if we're at the end of the array...
  if (ExhaustReadIndex >= numReadings) {
    // ...wrap around to the beginning:
    ExhaustReadIndex = 0;
  }
//Calculate the Average
 return (ExhaustTotal / numReadings * 9.0 / 5.0) + 32.0;
;

}
//void BoostPressureRead() {

//#define BoostPressurePin A14

//Pressure Transducer constants defined in setup
//const int BoostPTransMax = 600; //Boost PTrans = 0-60psi, scaled 10x for final resolution

//float BoostPressure = 0.1 * map(analogRead(BoostPressurePin), PTransMin, PTransMax, 0, BoostPTransMax); //MAP function only does integers... Scale range 10x and divide by 10 outside of function to get a decimal place back.


//}
/*----------------------------------------------------------------------
                          Fuel Pressure Alarm Function
------------------------------------------------------------------------*/
/*SSD1306 FUnction
void FuelPressureAlarm(float FuelPressure, unsigned long currentMillis) {
static int FlashPhase =0;
if (FuelPressure < 5.0) { // Sets FP threshold
if (FuelLowStartTime == 0) {
FuelLowStartTime = currentMillis;
} else if (currentMillis - FuelLowStartTime >= 1500) { // Threshold time for FP to be low before alarming 
FuelAlarmActive = true;
}
} else {
FuelLowStartTime = 0;
FuelAlarmActive = false;
}

if (FuelAlarmActive) {
if (currentMillis - LastFuelFlashTime >= 500) { //Flash Rate
LastFuelFlashTime = currentMillis;
FlashPhase = (FlashPhase + 1) %4; //to alternate between "FUEL" and reading
}

display.clearDisplay();
if (ShowFuelText) {
display.setTextSize(4);
display.setTextColor(SSD1306_WHITE);

if (FlashPhase ==0) {
display.setCursor(20, 15);
display.println("FUEL");
}
else if (FlashPhase == 2){
  display.setCursor(30, 15);
  display.print(FuelPressure,1);
}

}


//Draw Border for Alarm
display.fillRect(0,0,128,5,WHITE); //Top Border
display.fillRect(123,0,128,64,WHITE); //Right Border
display.fillRect(0,59,128,64,WHITE); //Bottom Border
display.fillRect(0,0,5,64,WHITE); //Left Border

}
display.display();
}
*/

void FuelPressureAlarm(float FuelPressure, unsigned long currentMillis) {
static int FlashPhase = 0;

// Check if fuel pressure is below threshold
if (FuelPressure < 5.0) {
if (FuelLowStartTime == 0) {
FuelLowStartTime = currentMillis;
} else if (currentMillis - FuelLowStartTime >= 1500) {
FuelAlarmActive = true;
}
} else {
FuelLowStartTime = 0;
FuelAlarmActive = false;
}

// Handle flashing logic
if (FuelAlarmActive) {
if (currentMillis - LastFuelFlashTime >= 500) {
LastFuelFlashTime = currentMillis;
FlashPhase = (FlashPhase + 1) % 4;
}

u8g2.clearBuffer(); // Clear internal buffer

if (ShowFuelText) {
u8g2.setDrawColor(1); // White
u8g2.setFont(u8g2_font_logisoso32_tf); // Large font

if (FlashPhase == 0) {
u8g2.setCursor(20, 40);
u8g2.print("FUEL");
} else if (FlashPhase == 2) {
u8g2.setCursor(30, 40);
u8g2.print(FuelPressure, 1);
}
}

// Draw Border for Alarm
u8g2.setDrawColor(1); // White
u8g2.drawBox(0, 0, 128, 5);// Top Border
u8g2.drawBox(123, 0, 5, 64);// Right Border
u8g2.drawBox(0, 59, 128, 5);// Bottom Border
u8g2.drawBox(0, 0, 5, 64);// Left Border

u8g2.sendBuffer(); // Push to screen
}
}

/*----------------------------------------------------------------------
                          Welcome Screen Function
------------------------------------------------------------------------*/

/* SSD1306 Welcome Screen
void WelcomeScreen() {
display.clearDisplay();
display.setTextSize(3);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("Cummins");
display.setCursor(0, 40);
display.println("Command");
display.display();
delay(7000); // Show for X seconds

/*Fade-Out Function
for (int y = 0; y <= SCREEN_HEIGHT; y += 2) {
display.fillRect(0, 0, SCREEN_WIDTH, y, SSD1306_BLACK);
display.setTextSize(3);
display.setTextColor(SSD1306_WHITE);
display.setCursor(0, 0);
display.println("Cummins");
display.setCursor(0, 40);
display.println("Command");
display.display();
delay(250); // Adjust for slower fade
}


display.clearDisplay();
display.display();
}
*/

void WelcomeScreen() {
u8g2.clearBuffer();
u8g2.setDrawColor(1); // White text
u8g2.setFont(u8g2_font_logisoso24_tf); // Large, bold font

u8g2.setCursor(0, 26); // Adjusted for baseline
u8g2.print("Cummins");

u8g2.setCursor(0, 60);
u8g2.print("Command");

u8g2.sendBuffer();
delay(7000); // Show for 7 seconds

// Optional Fade-Out Effect
for (int y = 0; y <= SCREEN_HEIGHT; y += 2) {
u8g2.clearBuffer();
u8g2.setDrawColor(1);
u8g2.setFont(u8g2_font_logisoso24_tf);
u8g2.setCursor(0, 26);
u8g2.print("Cummins");
u8g2.setCursor(0, 60);
u8g2.print("Command");

// Draw black rectangle to simulate fade
u8g2.setDrawColor(0); // Black
u8g2.drawBox(0, 0, SCREEN_WIDTH, y);

u8g2.sendBuffer();
delay(250); // Adjust for fade speed
}

u8g2.clearBuffer();
u8g2.sendBuffer();
}

/*----------------------------------------------------------------------
                             Screen Brightness Function
------------------------------------------------------------------------
void AdjustBrightness(){
  yValue = analogRead(JOY_Y_PIN);
  if (CurrentScreen != 3) return;

if (millis() - LastAdjustTime > AdjustDelay) {
if (yValue < 200 && ScreenBrightness < 10) {
ScreenBrightness++;
LastAdjustTime = millis();
} else if (yValue > 800 && ScreenBrightness > 0) {
ScreenBrightness--;
LastAdjustTime = millis();
}

// Apply brightness (contrast) to the display
int contrastValue = map(ScreenBrightness, 0, 10, 0, 255);
display.ssd1306_command(SSD1306_SETCONTRAST);
display.ssd1306_command(contrastValue);
}
}
*/
/*----------------------------------------------------------------------
                             Screen Navigation Function
------------------------------------------------------------------------*/
void ScreenNavigation() {
  xValue = analogRead(JOY_X_PIN);
if (millis() - LastNavTime > NavDelay){
  if (xValue <200) {
    CurrentScreen = max(0, CurrentScreen -1);
    LastNavTime = millis();
  } else if (xValue >800){
    CurrentScreen = min(3, CurrentScreen +1); //Declares 4 screens (0-3) Total
    LastNavTime = millis();
  }
}
}
/*----------------------------------------------------------------------
                             Screen Render Function
------------------------------------------------------------------------*/
/* SSD1306 Render
void RenderCurrentScreen(float ExhaustTempDisplay, float FuelPressureDisplay) {
display.clearDisplay();
display.setTextColor(SSD1306_WHITE);

switch (CurrentScreen) {
case 0:
// Screen 0: EGT and Fuel Pressure
display.setCursor(0, 6);
display.setTextSize(2);
display.print("EGT:");
display.print(ExhaustTempDisplay, 0);
display.print((char)247); // Degree symbol
display.print("F");

display.setCursor(0, 44);
display.print("FP:");
display.print(FuelPressureDisplay, 1);
display.print("PSI");

display.drawLine(0, 31, 127, 31, SSD1306_WHITE);
break;

case 1:
display.setCursor(10, 20);
display.setTextSize(2);
display.print("Screen 2");
break;

case 2:
display.setCursor(10, 20);
display.setTextSize(2);
display.print("Screen 3");
break;

case 3:
display.setCursor(0, 10);
display.setTextSize(2);
display.print("Brightness");
display.setTextSize(2);
display.setCursor(52, 35);
display.print(ScreenBrightness);
break;
}

display.display();
}
*/

void RenderCurrentScreen(float ExhaustTempDisplay, float FuelPressureDisplay) {
u8g2.clearBuffer(); // Clear internal memory
u8g2.setDrawColor(1); // Set color to white

switch (CurrentScreen) {
case 0:
// Screen 0: EGT and Fuel Pressure
u8g2.setFont(u8g2_font_ncenB08_tr); // Choose a readable font
u8g2.setCursor(0, 16);
u8g2.print("EGT:");
u8g2.print((int)ExhaustTempDisplay);
u8g2.print((char)176); // Degree symbol
u8g2.print("F");

u8g2.setCursor(0, 48);
u8g2.print("FP:");
u8g2.print(FuelPressureDisplay, 1);
u8g2.print("PSI");

u8g2.drawHLine(0, 32, 128); // Horizontal line
break;

case 1:
u8g2.setFont(u8g2_font_ncenB14_tr);
u8g2.setCursor(10, 32);
u8g2.print("Screen 2");
break;

case 2:
u8g2.setFont(u8g2_font_ncenB14_tr);
u8g2.setCursor(10, 32);
u8g2.print("Screen 3");
break;

case 3:
u8g2.setFont(u8g2_font_ncenB08_tr);
u8g2.setCursor(0, 16);
u8g2.print("Brightness");

u8g2.setFont(u8g2_font_ncenB14_tr);
u8g2.setCursor(52, 48);
u8g2.print(ScreenBrightness);
break;
}

u8g2.sendBuffer(); // Transfer internal memory to the display
}