#include <Arduino.h>
#include <U8g2lib.h>
#include <Wire.h> // library requires for IIC communication

// Display constructor
// =========================================================
//
//    CHANGE TO CORRECT DISPLAY CONSTRUCTOR
//
// =========================================================
U8G2_SH1107_128X128_1_HW_I2C u8g2(U8G2_R0);

// PWM input pins
// =========================================================
//
//    CHANGE TO CORRECT INPUT PINS - interruptible pins
//
// =========================================================
const int lambdaPin = 3;
const int gearPin = 5;

// Variables for storing timestamp and duration
volatile unsigned long lambdaStartTime = 0;
volatile unsigned long lambdaEndTime = 0;
volatile bool lambdaCaptureFlag = false;
volatile unsigned long gearStartTime = 0;
volatile unsigned long gearEndTime = 0;
volatile bool gearCaptureFlag = false;

// PWM pulse uptime percentage
float lambdaUptimePercentage = 0.0;
float gearUptimePercentage = 0.0;

// START
void setup() {
  u8g2.begin();
  u8g2.setContrast(255);
  /*pinMode(lambdaPin, INPUT);
  pinMode(gearPin, INPUT);

  // Attach interrupts for lambda and gear pins
  attachInterrupt(digitalPinToInterrupt(lambdaPin), lambdaISR, CHANGE);
  attachInterrupt(digitalPinToInterrupt(gearPin), gearISR, CHANGE);*/
}

// UPDATE
void loop() {
  /*// Check if lambda value has been captured
  if (lambdaCaptureFlag) {
    lambdaCaptureFlag = false;  // Reset flag
    calculateLambdaUptimePercentage();  // Calculate lambda uptime percentage
    updateDisplay();  // Update display with new values
  }
  
  // Check if gear value has been captured
  if (gearCaptureFlag) {
    gearCaptureFlag = false;  // Reset flag
    calculateGearUptimePercentage();  // Calculate gear uptime percentage
    updateDisplay();  // Update display with new values
  }*/
  
    displayScreen("0.842", 31, "4");
}

// Lambda ISR
void lambdaISR() {
  if (digitalRead(lambdaPin) == HIGH) {
    lambdaStartTime = micros();  // Record start time of high pulse
  } else {
    lambdaEndTime = micros();    // Record end time of high pulse
    lambdaCaptureFlag = true;    // Set flag indicating capture is complete
  }
}

// Gear ISR
void gearISR() {
  if (digitalRead(gearPin) == HIGH) {
    gearStartTime = micros();    // Record start time of high pulse
  } else {
    gearEndTime = micros();      // Record end time of high pulse
    gearCaptureFlag = true;      // Set flag indicating capture is complete
  }
}

// Calculate lambda uptime percentage
void calculateLambdaUptimePercentage() {
  unsigned long cycleDuration = lambdaEndTime - lambdaStartTime;
  unsigned long totalPeriod = millis() - lambdaStartTime;
  
  // Calculate uptime percentage
  lambdaUptimePercentage = (float)cycleDuration / (float)totalPeriod * 100.0;
}

// Calculate gear uptime percentage
void calculateGearUptimePercentage() {
  unsigned long cycleDuration = gearEndTime - gearStartTime;
  unsigned long totalPeriod = millis() - gearStartTime;
  
  // Calculate uptime percentage
  gearUptimePercentage = (float)cycleDuration / (float)totalPeriod * 100.0;
}

// Update display with current values
void updateDisplay() {
 
  // Map lambda and gear values from PWM uptime percentages
  float lambda = calcLambdaValue();
  int gear = calcGearValue();

  // Convert values to strings
  // Lambda
  char lambdaStr[6];
  dtostrf(lambda, 1, 3, lambdaStr);
  // Gear
  char gearStr[2];
  if (gear <= 0) { // if gear equals 0 display a dash
    strcpy(gearStr, "-");
  } else {
    itoa(gear, gearStr, 10);
  }
  
  // Calculate dynamic bar width based on lambda value
  uint8_t barWidth = calculateBarWidth(lambda);
  
  // Draw graphics
  displayScreen(lambdaStr, barWidth, gearStr);
}

// Draw graphics
void displayScreen(const char* lambdaValue, uint8_t barWidth, const char* gearValue) {
  u8g2.firstPage(); // select the first page of the display (page is 128x8px), since we are using the page drawing method of the u8g2 library
  do { // draw
    u8g2.clearBuffer();
    u8g2.setFontMode(1);
    u8g2.setBitmapMode(1);
    
    // Lambda
    u8g2.setFont(u8g2_font_profont17_tr);
    u8g2.drawStr(39, 17, "Lambda"); // text
    u8g2.setFont(u8g2_font_profont29_tr);
    u8g2.drawStr(25, 42, lambdaValue); // value
    
    // Box
    u8g2.drawFrame(1, 50, 126, 22); // frame
    u8g2.drawBox(3, 52, barWidth, 18); // fill
    u8g2.drawLine(3, 48, 3, 44); // verticals
    u8g2.drawLine(124, 48, 124, 44);
    u8g2.drawLine(50, 48, 50, 44);
    
    // Gear
    u8g2.setFont(u8g2_font_profont22_tr);
    u8g2.drawStr(44, 92, "Gear"); // text
    u8g2.setFont(u8g2_font_profont29_tr);
    u8g2.drawStr(58, 122, gearValue); // value
    
    u8g2.sendBuffer();
    

  } while ( u8g2.nextPage() );
}

// Map lambda value to bar width
uint8_t calculateBarWidth(float lambda) {
  return map(lambda * 1000, 600, 1700, 2, 122);
}

// Calc lambda value
// =========================================================
//
//    CHANGE TO CORRECT VALUE MAPPING
//
// =========================================================
float calcLambdaValue() {
  return map(lambdaUptimePercentage, 5, 95, 600, 1700) / 1000.0;
}

// Calc gear value
// =========================================================
//
//    CHANGE TO CORRECT VALUE MAPPING
//
// =========================================================
int calcGearValue() {
  return map(gearUptimePercentage, 0, 100, 0, 5);
}