#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <ezButton.h>
#include <EEPROM.h>
#include <SoftwareSerial.h>

SoftwareSerial hc06(2,3); //HC06 rx pin 3 - tx pin 2

const int FL_UP_RELAY = 13; //valve add pressure D13
const int FL_DOWN_RELAY = 12; //valve remove pressure D12

const int FR_UP_RELAY = 11; //valve add pressure D11
const int FR_DOWN_RELAY = 10; //valve remove pressure D10

const int RL_UP_RELAY = 9; //valve add pressure D9
const int RL_DOWN_RELAY = 8; //valve remove pressure D8

const int RR_UP_RELAY = 7; //valve add pressure D7
const int RR_DOWN_RELAY = 6; //valve remove pressure D6

// OLED FeatherWing buttons map to different pins depending on board.
// The I2C (Wire) bus may also be different.
#if defined(ESP8266)
  #define BUTTON_A  0
  #define BUTTON_B 16
  #define BUTTON_C  2
  #define WIRE Wire
#elif defined(ESP32)
  #define BUTTON_A 15
  #define BUTTON_B 32
  #define BUTTON_C 14
  #define WIRE Wire
#elif defined(ARDUINO_STM32_FEATHER)
  #define BUTTON_A PA15
  #define BUTTON_B PC7
  #define BUTTON_C PC5
  #define WIRE Wire
#elif defined(TEENSYDUINO)
  #define BUTTON_A  4
  #define BUTTON_B  3
  #define BUTTON_C  8
  #define WIRE Wire
#elif defined(ARDUINO_FEATHER52832)
  #define BUTTON_A 31
  #define BUTTON_B 30
  #define BUTTON_C 27
  #define WIRE Wire
#elif defined(ARDUINO_ADAFRUIT_FEATHER_RP2040)
  #define BUTTON_A  9
  #define BUTTON_B  8
  #define BUTTON_C  7
  #define WIRE Wire1
#else // 32u4, M0, M4, nrf52840 and 328p
  #define BUTTON_A  9
  #define BUTTON_B  6
  #define BUTTON_C  5
  #define WIRE Wire
#endif

#define BLUE 0x001F
#define RED 0xF800
#define GREEN 0x07E0
#define CYAN 0x07FF
#define YELLOW 0xFFE0

Adafruit_SSD1306 display = Adafruit_SSD1306(128, 64, &WIRE);

ezButton button_savepreset1(5); //green save preset 1 D5
ezButton button_gopreset1(4); //blue go preset 1 button D4
ezButton button_groundmode(3); //yellow go ground button D3
ezButton button_cancel(2); //red cancel button D2

char Data; // the data received

void setup() 
{
  Serial.begin(115200);
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // Address 0x3C for 128x32
  hc06.begin(115200);

  pinMode( FL_UP_RELAY, OUTPUT);
  pinMode( FL_DOWN_RELAY, OUTPUT);
  pinMode( FR_UP_RELAY, OUTPUT);
  pinMode( FR_DOWN_RELAY, OUTPUT);
  pinMode( RL_UP_RELAY, OUTPUT);
  pinMode( RL_DOWN_RELAY, OUTPUT);
  pinMode( RR_UP_RELAY, OUTPUT);
  pinMode( RR_DOWN_RELAY, OUTPUT);
  
  gopreset1();
  delay(2000);
}

void loop() 
{
  button_savepreset1.loop();
  button_gopreset1.loop(); 
  button_groundmode.loop(); 
  button_cancel.loop(); // MUST call the loop() function first
  readpressure();
  
  digitalWrite(FL_UP_RELAY, HIGH);
  digitalWrite(FL_DOWN_RELAY, HIGH);
  digitalWrite(FR_UP_RELAY, HIGH);
  digitalWrite(FR_DOWN_RELAY, HIGH);
  
  digitalWrite(RL_UP_RELAY, HIGH);
  digitalWrite(RL_DOWN_RELAY, HIGH);
  digitalWrite(RR_UP_RELAY, HIGH);
  digitalWrite(RR_DOWN_RELAY, HIGH);
  //read from phyisicals buttons
  { 
    if(button_savepreset1.isPressed())
    {
    savepreset1();
    }
    if(button_gopreset1.isPressed())
    {
    gopreset1();
    }

    if(button_groundmode.isPressed())
    {
    ground();
    }  
  }

//read from console input
    if (Serial.available() > 0) 
    {
      Data = Serial.read();

      switch (Data) 
      { 
      case 'S':
        savepreset1();
        break;
        
      case 'P':
        gopreset1();
        break;
      
      case 'G':
        Serial.println("G");
        ground();
        break;
      
      case '2':
        Serial.println("FL UP");
        break;

      case '3':
        Serial.println("FL DOWN");
        break;

      case '4':
        Serial.println("FR UP");
        break;

      case '5':
        Serial.println("FR DOWN");
        break;

      case '6':
        Serial.println("RL UP");
        break;

      case '7':
        Serial.println("RL DOWN");
        break;

      case '8':
        Serial.println("RR UP");
        break;

      case '9':
        Serial.println("RR DOWN");
        break;

      }
  }  

   //read from BT controller butons
    if (hc06.available() > 0) 
    {
      Data = hc06.read();

      switch (Data) 
      { 
      case 'S':
        Serial.println("SAVE PRESET 1");
        savepreset1();
        break;
        
      case 'P':
        Serial.println("GO PRESET 1");
        gopreset1();
        break;

      case 'G':
        Serial.println("GROUND");
        ground();
        break;
        
      case '2':
        Serial.println("FL UP");
        digitalWrite(FL_UP_RELAY, LOW);
        break;

      case '3':
        Serial.println("FL DOWN");
        digitalWrite(FL_DOWN_RELAY, LOW);
        break;

      case '4':
        Serial.println("FR UP");
        digitalWrite(FR_UP_RELAY, LOW);
        break;

      case '5':
        Serial.println("FR DOWN");
        digitalWrite(FR_DOWN_RELAY, LOW);
        break;

      case '6':
        Serial.println("RL UP");
        digitalWrite(RL_UP_RELAY, LOW);
        break;

      case '7':
        Serial.println("RL DOWN");
        digitalWrite(RL_DOWN_RELAY, LOW);
        break;

      case '8':
        Serial.println("RR UP");
        digitalWrite(RR_UP_RELAY, LOW);
        break;

      case '9':
        Serial.println("RR DOWN");
        digitalWrite(RR_DOWN_RELAY, LOW);
        break;
      }
  }


  display.display();
}

void readpressure()
{
  int frontLeft_pressure=(((3.0*(((int(analogRead(A0))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
  int frontRight_pressure=(((3.0*(((int(analogRead(A1))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
  int rearLeft_pressure=(((3.0*(((int(analogRead(A2))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
  int rearRight_pressure=(((3.0*(((int(analogRead(A3))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;

  display.setTextSize(2);
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  //drawcross
  display.drawLine(0, 32, 128, 32, WHITE);
  display.drawLine(64, 0, 64, 64, WHITE);
  //BAG1
  display.setCursor(10, 10);
  display.print(frontLeft_pressure);
  //BAG2
  display.setCursor(80, 10);
  display.print(frontRight_pressure);
  //BAG3
  display.setCursor(10, 40);
  display.print(rearLeft_pressure);
  //BAG4
  display.setCursor(80, 40);
  display.print(rearRight_pressure);
  display.display();
}

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

  int frontLeft_pressure=(((3.0*(((int(analogRead(A0))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
  int frontRight_pressure=(((3.0*(((int(analogRead(A1))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
  int rearLeft_pressure=(((3.0*(((int(analogRead(A2))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
  int rearRight_pressure=(((3.0*(((int(analogRead(A3))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;

  EEPROM.write(1,frontLeft_pressure); //save front left bag pressure
  EEPROM.write(2,frontRight_pressure); //save front right bag pressure
  EEPROM.write(3,rearLeft_pressure); //save rear left bag 
  EEPROM.write(4,rearRight_pressure); //save rear right bag

  display.setCursor(30, 32);
  display.print("Save Preset 1 ");
  display.display();
  delay(1000);
  display.clearDisplay();
  int preset1FrontLeft_pressure=EEPROM.read(1); //pression FL en mode ride stocke case 1
  int preset1FrontRight_pressure=EEPROM.read(2); //FR
  int preset1RearLeft_pressure=EEPROM.read(3); //RL
  int preset1RearRight_pressure=EEPROM.read(4);  //RL
  display.setTextSize(2);
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.drawLine(0, 32, 128, 32, WHITE);
  display.drawLine(64, 0, 64, 64, WHITE);
  //BAG1
  display.setCursor(10, 10);
  display.print(frontLeft_pressure);
  //BAG2
  display.setCursor(80, 10);
  display.print(frontRight_pressure);
  //BAG3
  display.setCursor(10, 40);
  display.print(rearLeft_pressure);
  //BAG4
  display.setCursor(80, 40);
  display.print(rearRight_pressure);
  display.display();
  delay(2000);
}

void gopreset1()
{
  display.setTextSize(1);
  display.clearDisplay();
  display.setTextColor(SSD1306_WHITE);
  display.drawLine(0, 32, 128, 32, WHITE);
  display.drawLine(64, 0, 64, 64, WHITE);
  //store eeprom value as preset1 settings
  int preset1FrontLeft_pressure=EEPROM.read(1); //pression FL en mode ride stocke case 1
  int preset1FrontRight_pressure=EEPROM.read(2); //FR
  int preset1RearLeft_pressure=EEPROM.read(3); //RL
  int preset1RearRight_pressure=EEPROM.read(4);  //RL
  //display preset values
  int frontLeft_pressure=(((3.0*(((int(analogRead(A0))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
  int frontRight_pressure=(((3.0*(((int(analogRead(A1))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
  int rearLeft_pressure=(((3.0*(((int(analogRead(A2))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
  int rearRight_pressure=(((3.0*(((int(analogRead(A3))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
  delay(2000);
  
  //if pressure not correct
  if(preset1FrontLeft_pressure != 255 & preset1FrontRight_pressure != 255 & preset1RearLeft_pressure != 255 & preset1RearRight_pressure != 255 )
  {
    while(frontLeft_pressure !=preset1FrontLeft_pressure || frontRight_pressure !=preset1FrontRight_pressure || rearLeft_pressure !=preset1RearLeft_pressure || rearRight_pressure !=preset1RearRight_pressure)
    {
      int frontLeft_pressure=(((3.0*(((int(analogRead(A0))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
      int frontRight_pressure=(((3.0*(((int(analogRead(A1))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
      int rearLeft_pressure=(((3.0*(((int(analogRead(A2))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
      int rearRight_pressure=(((3.0*(((int(analogRead(A3))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;

      display.clearDisplay();
      display.setTextSize(2);
      display.setCursor(5,0);
      display.print(preset1FrontLeft_pressure);
      display.setTextSize(1);
      display.setCursor(15,20);
      display.print(frontLeft_pressure);

      display.setTextSize(2);
      display.setCursor(80,0);
      display.print(preset1FrontRight_pressure);
      display.setTextSize(1);
      display.setCursor(85,20);
      display.print(frontRight_pressure);

      display.setTextSize(2);
      display.setCursor(5,32);
      display.print(preset1RearLeft_pressure);
      display.setTextSize(1);
      display.setCursor(15,52);
      display.print(rearLeft_pressure);

      display.setTextSize(2);
      display.setCursor(80,32);
      display.print(preset1RearRight_pressure);
      display.setTextSize(1);
      display.setCursor(85,52);
      display.print(rearRight_pressure);

      display.display();     
      
    //if cancel, close all
    button_cancel.loop(); // MUST call the loop() function first
    if(button_cancel.isPressed())  
    {
      display.clearDisplay();
      digitalWrite(FL_UP_RELAY, HIGH);
      digitalWrite(FR_UP_RELAY, HIGH);
      digitalWrite(RL_UP_RELAY, HIGH);
      digitalWrite(RR_UP_RELAY, HIGH);
      digitalWrite(FL_DOWN_RELAY, HIGH);
      digitalWrite(FR_DOWN_RELAY, HIGH);
      digitalWrite(RL_DOWN_RELAY, HIGH);
      digitalWrite(RR_DOWN_RELAY, HIGH);
      break;
    }

    if (Serial.available() >0)
      {
        if (Serial.read() == 'x')
        {
          break;
        }
      }

    if (hc06.available() >0)
      {
        if (hc06.read() == 'x')
        {
          break;
        }
      }

      //if too much pressure in FL release air
      if(frontLeft_pressure>preset1FrontLeft_pressure+2)
      {
        digitalWrite(FL_UP_RELAY, HIGH); //stop add air
        digitalWrite(FL_DOWN_RELAY, LOW); //release air 
      }
      //if too low pressure in FL add air
      if (frontLeft_pressure<=preset1FrontLeft_pressure-1)
      {
        digitalWrite(FL_DOWN_RELAY, HIGH); //stop release air
        digitalWrite(FL_UP_RELAY, LOW); //add air 
      }
      //if too much pressure in FR release air
      if(frontRight_pressure>preset1FrontRight_pressure+2)
      {
        digitalWrite(FR_UP_RELAY, HIGH); //stop add air
        digitalWrite(FR_DOWN_RELAY, LOW); //release air 
      }
      //if too low pressure in FR add air
      if (frontRight_pressure<=preset1FrontRight_pressure-1)
      {
        digitalWrite(FR_DOWN_RELAY, HIGH); //stop release air
        digitalWrite(FR_UP_RELAY, LOW); //add air 
      }
      //if too much pressure in RL release air
      if(rearLeft_pressure>preset1RearLeft_pressure+2)
      {
        digitalWrite(RL_UP_RELAY, HIGH); //stop add air
        digitalWrite(RL_DOWN_RELAY, LOW); //release air 
      }
      //if too low pressure in RL add air
      if (rearLeft_pressure<=preset1RearLeft_pressure-1)
      {
        digitalWrite(RL_DOWN_RELAY, HIGH); //stop release air
        digitalWrite(RL_UP_RELAY, LOW); //add air 
      }
      //if too much pressure in RR release air
      if(rearRight_pressure>preset1RearRight_pressure+2)
      {
        digitalWrite(RR_UP_RELAY, HIGH); //stop add air
        digitalWrite(RR_DOWN_RELAY, LOW); //release air 
      }
      //if too low pressure in RR add air
      if (rearRight_pressure<=preset1RearRight_pressure-1)
      {
        digitalWrite(RR_DOWN_RELAY, LOW); //stop release air
        digitalWrite(RR_UP_RELAY, HIGH); //add air 
      }

      if (((frontLeft_pressure >=preset1FrontLeft_pressure-1) && (frontLeft_pressure<=preset1FrontLeft_pressure+1)) && ((frontRight_pressure >=preset1FrontRight_pressure-1) && (frontRight_pressure<=preset1FrontRight_pressure+1)) && ((rearLeft_pressure >=preset1RearLeft_pressure-1) && (rearLeft_pressure<=preset1RearLeft_pressure+1)) && ((rearRight_pressure >=preset1RearRight_pressure-1) && (rearRight_pressure<=preset1RearRight_pressure+1)))
      {
        display.clearDisplay();
        digitalWrite(FL_UP_RELAY, HIGH);
        digitalWrite(FR_UP_RELAY, HIGH);
        digitalWrite(RL_UP_RELAY, HIGH);
        digitalWrite(RR_UP_RELAY, HIGH);
        digitalWrite(FL_DOWN_RELAY, HIGH);
        digitalWrite(FR_DOWN_RELAY, HIGH);
        digitalWrite(RL_DOWN_RELAY, HIGH);
        digitalWrite(RR_DOWN_RELAY, HIGH);
        break;
      }
    }
  }
  else
  {
    display.setTextSize(1);
    display.clearDisplay();
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(10, 32);
    display.print("No Preset Saved ");
    display.display();
    delay(1000);
  }
}

void ground()
{
  int frontLeft_pressure=(((3.0*(((int(analogRead(A0))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
  int frontRight_pressure=(((3.0*(((int(analogRead(A1))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
  int rearLeft_pressure=(((3.0*(((int(analogRead(A2))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
  int rearRight_pressure=(((3.0*(((int(analogRead(A3))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;

  while(frontLeft_pressure !=5 || frontRight_pressure !=5 || rearLeft_pressure !=5 || rearRight_pressure !=5)
  {
    int frontLeft_pressure=(((3.0*(((int(analogRead(A0))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
    int frontRight_pressure=(((3.0*(((int(analogRead(A1))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
    int rearLeft_pressure=(((3.0*(((int(analogRead(A2))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;
    int rearRight_pressure=(((3.0*(((int(analogRead(A3))*5.0)/1024.0)-0.47)*1000000.0))/10e5)*14.5;

    display.clearDisplay();
    display.setTextSize(2);
    display.setCursor(5,0);
    display.print(5);
    display.setTextSize(1);
    display.setCursor(15,20);
    display.print(frontLeft_pressure);

    display.setTextSize(2);
    display.setCursor(80,0);
    display.print(5);
    display.setTextSize(1);
    display.setCursor(85,20);
    display.print(frontRight_pressure);

    display.setTextSize(2);
    display.setCursor(5,32);
    display.print(5);
    display.setTextSize(1);
    display.setCursor(15,52);
    display.print(rearLeft_pressure);

    display.setTextSize(2);
    display.setCursor(80,32);
    display.print(5);
    display.setTextSize(1);
    display.setCursor(85,52);
    display.print(rearRight_pressure);

    display.display();

    
    Serial.println(frontLeft_pressure);
    Serial.println(frontRight_pressure);
    Serial.println(rearLeft_pressure);
    Serial.println(rearRight_pressure);

    //open all release air valve
    digitalWrite(FL_UP_RELAY, HIGH);
    digitalWrite(FR_UP_RELAY, HIGH);
    digitalWrite(RL_UP_RELAY, HIGH);
    digitalWrite(RR_UP_RELAY, HIGH);
    digitalWrite(FL_DOWN_RELAY, LOW);
    digitalWrite(FR_DOWN_RELAY, LOW);
    digitalWrite(RL_DOWN_RELAY, LOW);
    digitalWrite(RR_DOWN_RELAY, LOW);

    //if cancel, close all
    button_cancel.loop(); // MUST call the loop() function first
    if(button_cancel.isPressed())  
    {
      display.clearDisplay();
      digitalWrite(FL_UP_RELAY, HIGH);
      digitalWrite(FR_UP_RELAY, HIGH);
      digitalWrite(RL_UP_RELAY, HIGH);
      digitalWrite(RR_UP_RELAY, HIGH);
      digitalWrite(FL_DOWN_RELAY, HIGH);
      digitalWrite(FR_DOWN_RELAY, HIGH);
      digitalWrite(RL_DOWN_RELAY, HIGH);
      digitalWrite(RR_DOWN_RELAY, HIGH);
      break;
    }

    if (Serial.available() >0)
      {
        if (Serial.read() == 'x')
        {
          break;
        }
      }

    if (hc06.available() >0)
      {
        if (hc06.read() == 'x')
        {
          break;
        }
      }
 
    if (frontLeft_pressure<=6) 
    { 
      digitalWrite(FL_UP_RELAY, HIGH);
      digitalWrite(FL_DOWN_RELAY, HIGH);
    }

    if (frontRight_pressure<=6) 
    {
      digitalWrite(FR_UP_RELAY, HIGH);
      digitalWrite(FR_DOWN_RELAY, HIGH);
    }

    if (rearLeft_pressure<=6) 
    {
      digitalWrite(RL_UP_RELAY, HIGH);
      digitalWrite(RL_DOWN_RELAY, HIGH);
    }

    if (rearRight_pressure<=6) 
    {
      digitalWrite(RR_UP_RELAY, HIGH);
      digitalWrite(RR_DOWN_RELAY, HIGH);
    }

    if ((frontLeft_pressure<=6) && (frontRight_pressure<=6) && (rearLeft_pressure<=6) && (rearRight_pressure<=6))
    {
    display.clearDisplay();  
    digitalWrite(FL_UP_RELAY, HIGH);
    digitalWrite(FR_UP_RELAY, HIGH);
    digitalWrite(RL_UP_RELAY, HIGH);
    digitalWrite(RR_UP_RELAY, HIGH);
    digitalWrite(FL_DOWN_RELAY, HIGH);
    digitalWrite(FR_DOWN_RELAY, HIGH);
    digitalWrite(RL_DOWN_RELAY, HIGH);
    digitalWrite(RR_DOWN_RELAY, HIGH);
    break;
    }

  }
}