#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <ezButton.h>
#include <EEPROM.h>
#if defined(ARDUINO) && ARDUINO >= 100
#define printByte(args) write(args);
#else
#define printByte(args) print(args, BYTE);
#endif
// Define pin numbers

int menu = 4;
int up = 5;
int down = 6;
int ok = 7;
int relay1 = 10;
int relay2 = 12;

// Initialize ezButton instances
ezButton buttonup(up);
ezButton buttondown(down);
ezButton buttonmenu(menu);
ezButton buttonok(ok);

// Variables to store pressure readings and settings
float p1;
float p2;

// Variables to store previous pressure readings
float oldp1 = 0;
float oldp2 = 0;

// Settings for pressure limits and intervals
float set_dp = 0;
float set_lp = 0;
float set_iv = 0;
float set_op = 0;
float old_set_dp = 1;
float old_set_lp = 1;
float old_set_iv = 1;
float old_set_op = 1;

// Variables for menu navigation and state tracking
int count = 0;
int old_count = 0;
bool counter = false;
bool incrmtr = false;
bool confg = false;
bool save_mode = false;
bool save_screen = false;

// LCD setup
LiquidCrystal_I2C lcd(0x27, 20, 4);

// Custom character for delta symbol
byte delta[] = {B00000, B00000, B00000, B00000, B00100, B01110, B11111, B00000};

int backlightState = LOW;
unsigned long previousMillis = 0;
const long interval = 200;

void setup()
{
  Serial.begin(115200);
  Serial.println("Reading from EEPROM");

  // Read settings from EEPROM
  set_dp = EEPROM.read(0) / 10;
  set_lp = EEPROM.read(4) / 10;
  set_iv = EEPROM.read(8) / 10;
  set_op = EEPROM.read(12) / 10;

  // ... (Serial print statements for debugging)
  Serial.println("EEPROM Read back");
  Serial.println(EEPROM.read(0));
  Serial.println(EEPROM.read(4));
  Serial.println(EEPROM.read(8));
  Serial.println(EEPROM.read(12));
  Serial.println("Update");
  set_dp = EEPROM.read(0) / 10;
  set_lp = EEPROM.read(4) / 10;
  set_iv = EEPROM.read(8) / 10;
  set_op = EEPROM.read(12) / 10;
  Serial.println("Initialize");
  Serial.print("set_dp=");
  Serial.println(set_dp);
  Serial.print("set_lp=");
  Serial.println(set_lp);
  Serial.print("set_iv=");
  Serial.println(set_iv);
  Serial.print("set_op=");
  Serial.println(set_op);


  // Initialize pins
  pinMode(menu, INPUT);
  pinMode(up, INPUT);
  pinMode(down, INPUT);
  pinMode(ok, INPUT);

  pinMode(relay1, OUTPUT);
  pinMode(relay2, OUTPUT);

  // ... (LCD initialization and custom character setup)
  lcd.init();
  lcd.backlight();
  lcd.createChar(1, delta);
  lcd.home();

  // ... (Debugging initialization)
  ////////////////////////////debug////////////////////////////
  //p1 = 1.0;
  //p2 = 2.0;
  ////////////////////////////debug////////////////////////////
}

// Function to map a value from one range to another
float mapFloat(float x, float in_min, float in_max, float out_min, float out_max) {
  return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
void loop()
{
  // Update button states
  buttonup.loop();
  buttondown.loop();
  buttonmenu.loop();
  buttonok.loop();

  // Read pressure sensor values
  p1 = analogRead(A0);
  p2 = analogRead(A1);

  // Map pressure values to desired range
  p1 = mapFloat(p1, 102, 920, 0.0 , 6.89);
  p2 = mapFloat(p2, 102, 920, 0.0 , 6.89);
if(p1 < 0) p1 = 0;
if(p1 > 100) p1 = 100;

if(p2 < 0) p2 = 0;
if(p2 > 100) p2 = 100;
  // Call function to handle output based on pressure readings
  outputhandle();

  // ... (Code for updating LCD display and menu interaction)
  if (oldp1 != p1 || oldp2 != p2)
  {
    if (confg == false)
    {
      save_mode = false;
      lcd.home();
      lcd.setCursor(0, 0);
      lcd.print("P1 (IN):     bar");
      lcd.setCursor(8, 0);
      lcd.print(p1);
      lcd.setCursor(0, 1);
      lcd.print("P2(OUT):     bar");
      lcd.setCursor(8, 1);
      lcd.print(p2);

      lcd.setCursor(0, 2);
      lcd.print("Live ");
      lcd.setCursor(5, 2);
      lcd.printByte(1);
      lcd.setCursor(6, 2);
      lcd.print("P:     bar");
      lcd.setCursor(8, 2);
      if(p2 > p1) lcd.print(p2 - p1);
      if(p2 < p1) lcd.print(p1 - p2);

      lcd.setCursor(2, 3);
      lcd.printByte(1);
      lcd.setCursor(3, 3);
      lcd.print("P:");
      lcd.setCursor(5, 3);
      lcd.print(set_dp);

      lcd.setCursor(12, 3);
      lcd.print("LP:");
      lcd.setCursor(14, 3);
      lcd.print(set_lp);

      oldp1 = p1;
      oldp2 = p2;
    }
  }

  // ... (Code for handling menu navigation and setting adjustments)
  if (digitalRead(menu) == HIGH && confg == false)
  {
    confg = true;
    Serial.println("Menu Mode");
    lcd.backlight();
    lcd.clear(); ////////////
  }

  while (confg == true)
  {
    buttonup.loop();
    buttondown.loop();
    buttonmenu.loop();
    buttonok.loop();
    while (digitalRead(menu) == HIGH && counter == false)
    {
      buttonup.loop();
      buttondown.loop();
      buttonmenu.loop();
      buttonok.loop();
      counter = true;
      count++;
      if (count > 4)
      {
        Serial.println("Index reset");
        count = 1;
        old_set_dp = 999;
        old_set_lp = 999;
        old_set_iv = 999;
        old_set_op = 999;
      }
      if (old_count != count)
      {
        Serial.println(count);
        old_count = count;
      }
    }
    if (digitalRead(menu) == LOW)
    {
      counter = false;
    }

    if (count == 1 && save_mode == false)
    {
      if (buttonok.isPressed())
      {
        Serial.println("Saved!");
      }
      if (buttonup.isPressed())
      {
        set_dp = set_dp + 0.1;
        Serial.println("UP");
      }
      if (buttondown.isPressed())
      {
        set_dp = set_dp - 0.1;
        Serial.println("DOWN");
      }
      if (old_set_dp != set_dp)
      {
        Serial.println("Menu 1");
        // lcd.clear();
        lcd.setCursor(15, 0);
        lcd.print(" ");
        lcd.setCursor(15, 1);
        lcd.print(" ");
        lcd.setCursor(15, 2);
        lcd.print(" ");
        lcd.setCursor(15, 3);
        lcd.print(" ");

        lcd.setCursor(0, 0);
        lcd.print("Set P :");
        lcd.setCursor(7, 0);
        lcd.print(set_dp);
        lcd.setCursor(15, 0);
        lcd.print("<");

        lcd.setCursor(0, 1);
        lcd.print("Set LP:");
        lcd.setCursor(7, 1);
        lcd.print(set_lp);

        lcd.setCursor(0, 2);
        lcd.printByte(1);
        lcd.setCursor(1, 2);
        lcd.print("P(IV):");
        lcd.setCursor(7, 2);
        lcd.print(set_iv);

        lcd.setCursor(0, 3);
        lcd.printByte(1);
        lcd.setCursor(1, 3);
        lcd.print("P(0) :");
        lcd.setCursor(7, 3);
        lcd.print(set_op);

        old_set_dp = set_dp;
      }
    }

    if (count == 2 && save_mode == false)
    {
      if (buttonok.isPressed())
      {
        Serial.println("Saved!");
      }
      if (buttonup.isPressed())
      {
        set_lp = set_lp + 0.1;
        Serial.println("UP");
      }
      if (buttondown.isPressed())
      {
        set_lp = set_lp - 0.1;
        Serial.println("DOWN");
      }
      if (old_set_lp != set_lp)
      {
        Serial.println("Menu 2");
        // lcd.clear();
        lcd.setCursor(15, 0);
        lcd.print(" ");
        lcd.setCursor(15, 1);
        lcd.print(" ");
        lcd.setCursor(15, 2);
        lcd.print(" ");
        lcd.setCursor(15, 3);
        lcd.print(" ");

        lcd.setCursor(0, 0);
        lcd.print("Set P :");
        lcd.setCursor(7, 0);
        lcd.print(set_dp);

        lcd.setCursor(0, 1);
        lcd.print("Set LP:");
        lcd.setCursor(7, 1);
        lcd.print(set_lp);
        lcd.setCursor(15, 1);
        lcd.print("<");
        old_set_lp = set_lp;

        lcd.setCursor(0, 2);
        lcd.printByte(1);
        lcd.setCursor(1, 2);
        lcd.print("P(IV):");
        lcd.setCursor(7, 2);
        lcd.print(set_iv);

        lcd.setCursor(0, 3);
        lcd.printByte(1);
        lcd.setCursor(1, 3);
        lcd.print("P(0) :");
        lcd.setCursor(7, 3);
        lcd.print(set_op);
      }
    }

    if (count == 3 && save_mode == false)
    {
      if (buttonok.isPressed())
      {
        Serial.println("Saved!");
      }
      if (buttonup.isPressed())
      {
        set_iv = set_iv + 0.1;
        Serial.println("UP");
      }
      if (buttondown.isPressed())
      {
        set_iv = set_iv - 0.1;
        Serial.println("DOWN");
      }
      if (old_set_iv != set_iv)
      {
        Serial.println("Menu 3");
        // lcd.clear();
        lcd.setCursor(15, 0);
        lcd.print(" ");
        lcd.setCursor(15, 1);
        lcd.print(" ");
        lcd.setCursor(15, 2);
        lcd.print(" ");
        lcd.setCursor(15, 3);
        lcd.print(" ");

        lcd.setCursor(0, 0);
        lcd.print("Set P :");
        lcd.setCursor(7, 0);
        lcd.print(set_dp);

        lcd.setCursor(0, 1);
        lcd.print("Set LP:");
        lcd.setCursor(7, 1);
        lcd.print(set_lp);
        old_set_iv = set_iv;

        lcd.setCursor(0, 2);
        lcd.printByte(1);
        lcd.setCursor(1, 2);
        lcd.print("P(IV):");
        lcd.setCursor(7, 2);
        lcd.print(set_iv);
        lcd.setCursor(15, 2);
        lcd.print("<");

        lcd.setCursor(0, 3);
        lcd.printByte(1);
        lcd.setCursor(1, 3);
        lcd.print("P(0) :");
        lcd.setCursor(7, 3);
        lcd.print(set_op);
      }
    }

    if (count == 4 && save_mode == false)
    {
      if (buttonok.isPressed())
      {
        Serial.println("Saved!");
      }
      if (buttonup.isPressed())
      {
        set_op = set_op + 0.1;
        Serial.println("UP");
      }
      if (buttondown.isPressed())
      {
        set_op = set_op - 0.1;
        Serial.println("DOWN");
      }
      if (old_set_op != set_op)
      {
        Serial.println("Menu 4");
        // lcd.clear();
        lcd.setCursor(15, 0);
        lcd.print(" ");
        lcd.setCursor(15, 1);
        lcd.print(" ");
        lcd.setCursor(15, 2);
        lcd.print(" ");
        lcd.setCursor(15, 3);
        lcd.print(" ");

        lcd.setCursor(0, 0);
        lcd.print("Set P :");
        lcd.setCursor(7, 0);
        lcd.print(set_dp);

        lcd.setCursor(0, 1);
        lcd.print("Set LP:");
        lcd.setCursor(7, 1);
        lcd.print(set_lp);
        old_set_op = set_op;

        lcd.setCursor(0, 2);
        lcd.printByte(1);
        lcd.setCursor(1, 2);
        lcd.print("P(IV):");
        lcd.setCursor(7, 2);
        lcd.print(set_iv);

        lcd.setCursor(0, 3);
        lcd.printByte(1);
        lcd.setCursor(1, 3);
        lcd.print("P(0) :");
        lcd.setCursor(7, 3);
        lcd.print(set_op);
        lcd.setCursor(15, 3);
        lcd.print("<");
      }
    }

    if (buttonok.isPressed())
    {
      save_mode = true;
    }
    while (save_mode == true)
    {
      buttonup.loop();
      buttondown.loop();
      buttonmenu.loop();
      buttonok.loop();
      if (save_screen == false)
      {
        Serial.println("Disp");
        lcd.clear();
        lcd.setCursor(0, 0);
        lcd.print("Save the values?");
        lcd.setCursor(0, 2);
        lcd.print("Enter to Save");
        lcd.setCursor(0, 3);
        lcd.print("Menu to cancel");
        save_screen = true;
      }
      if (buttonok.isPressed())
      {
        lcd.clear();
        save_mode = false;
        save_screen = false;

        // EEPROM Update
        // set_dp = EEPROM.read(1) / 10;
        // set_lp = EEPROM.read(2) / 10;
        // set_iv = EEPROM.read(3) / 10;
        // set_op = EEPROM.read(4) / 10;
        // ... (Code for saving settings to EEPROM)
        EEPROM.update(0, set_dp * 10);
        EEPROM.update(4, set_lp * 10);
        EEPROM.update(8, set_iv * 10);
        EEPROM.update(12, set_op * 10);
        Serial.println();
        Serial.println();
        Serial.println("Updated values");
        Serial.print("set_dp=");
        Serial.print(set_dp);
        Serial.println();
        Serial.print("set_lp=");
        Serial.print(set_lp);
        Serial.println();
        Serial.print("set_iv=");
        Serial.print(set_iv);
        Serial.println();
        Serial.print("set_op=");
        Serial.print(set_op);
        Serial.println();

        Serial.println("Readback RAW");
        Serial.println(EEPROM.read(0));
        Serial.println(EEPROM.read(4));
        Serial.println(EEPROM.read(8));
        Serial.println(EEPROM.read(12));
        Serial.println();
        Serial.println();
        Serial.println("Saved!!!!!!!!!");

        confg = false;
        oldp1 = 9999;
        oldp2 = 9999;
      }
      if (buttonmenu.isPressed())
      {
        lcd.clear();
        Serial.println("Exit!!!");
        save_screen = false;
        save_mode = false;
        oldp1 = 9999;
        oldp2 = 9999;
        old_set_dp = 9999;
        old_set_iv = 9999;
        old_set_lp = 9999;
        old_set_op = 9999;
        break;
      }
    }
  }
}

void outputhandle()
{
  Serial.println("test");
  if (p2 - p1 > set_dp)
  {
    digitalWrite(relay1, HIGH); // Contactor
    digitalWrite(relay2, HIGH); // Contactor
    Serial.println("Contactor ON");
  }

  if (p2 - p1 > set_iv)
  {
    lcd.setCursor(0, 3);
    lcd.print("                    ");
    lcd.setCursor(5, 3);
    lcd.print("Warning!");

    unsigned long currentMillis = millis();

    if (currentMillis - previousMillis >= interval)
    {
      previousMillis = currentMillis;
      if (backlightState == LOW)
      {
        backlightState = HIGH;
        lcd.noBacklight();
      }
      else
      {
        backlightState = LOW;
        lcd.backlight();
      }
    }
  }

  // ... (Debugging code)
  ////////////////////////////debug////////////////////////////
  // p2 = p2 + 1;
  // Serial.println(p2);
  // delay(1000);
  /////////////////////////////debug////////////////////////////
}