#include <SPI.h>
#include <Wire.h>
#include <Adafruit_SSD1306.h>
#include <PID_v1.h>
#include <Bounce2.h>

const byte buttonPin0 = 2;
const byte buttonPin1 = 3;
const byte buttonPin2 = 4;
const byte buttonPin3 = 5;

////////////////////////////////////////////////////////////////////////////////////////////////////////////
// STATE MACHINE STATES
////////////////////////////////////////////////////////////////////////////////////////////////////////////
enum States {
  STOP,
  STARTING,
  RUN_AUTO,
  RUN_MANUAL
};

// Set the initial (i.e., starting) state
States state = States::STOP;

Bounce debouncer0 = Bounce();
Bounce debouncer1 = Bounce();
Bounce debouncer2 = Bounce();
Bounce debouncer3 = Bounce();
////////////////////////////////////////////////////////////////////////////////////////////////////////////



// Define variables
double temperature;
double desiredTemperature = 100.0;
double feederSpeed;
double blowerSpeed;
double oxygenLevel;
double targetOxygenLevel = 21.0;
bool isRunning = true;
bool manualMode = false;

// Define PID constants
const double feeder_Kp = 5.0;
const double feeder_Ki = 0.3;
const double feeder_Kd = 0.1;

const double blower_Kp = 5.0;
const double blower_Ki = 0.3;
const double blower_Kd = 0.1;

// Define PID objects
PID feederPID(&temperature, &feederSpeed, &desiredTemperature, feeder_Kp, feeder_Ki, feeder_Kd, 100);
PID blowerPID(&oxygenLevel, &blowerSpeed, &targetOxygenLevel, blower_Kp, blower_Ki, blower_Kd, 100);

// Define sensor and relay pins
const int temperaturePin = A0;
const int oxygenPin = A1;



////////////////////////////////////////////////////////////////
// FEEDER RELAY PIN  and  timer variables
////////////////////////////////////////////////////////////////
const int Feeder_Relay_pin = 7;
long Feeder_on_Duration = 2000;      // ON   time for Feeder_Relay  2 seconds
long Feeder_off_Duration = 20000;    // OFF  time for Feeder_Relay 20 seconds

long rememberTime1=0;               
int Feeder_Relay_State =LOW;      // initial state of Feeder_Relay
////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////
// BLOWER RELAY PIN  and  timer variables
////////////////////////////////////////////////////////////////
const int Blower_Relay_pin = 8;
long Blower_on_Duration = 2000;      // OFF time for Blower_Relay
long Blower_off_Duration = 2000;    // ON  time for Blower_Relay

long rememberTime2=0;               
int Blower_Relay_State =LOW;      // initial state of Blower_Relay
////////////////////////////////////////////////////////////////

// Define the display parameters
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

// Create an SSD1306 object
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);

// INTRODUCE THE TWO HELPER FUNCTIONS
volatile int buttonPressed = -1;

void changeState0() { buttonPressed = 0; }
void changeState1() { buttonPressed = 1; }
void changeState2() { buttonPressed = 2; }
void changeState3() { buttonPressed = 3; }

void handleButtonPress() {
  if (buttonPressed == 0) state = STOP;
  if (buttonPressed == 1) state = STARTING;
  if (buttonPressed == 2) state = RUN_AUTO;
  if (buttonPressed == 3) state = RUN_MANUAL;
  buttonPressed = -1;
  Serial.print("STATE: ");
  Serial.println(state);
}

void displayInfo(const char* stateName); // Function declaration

unsigned long currentMillis = 0;
unsigned long currentMillis2 = 0;

// SETUP
void setup() {

  //pinMode(BUTTON_PIN, INPUT_PULLUP); // Use internal pullup resistor for button
  pinMode(buttonPin0, INPUT_PULLUP);
  pinMode(buttonPin1, INPUT_PULLUP);
  pinMode(buttonPin2, INPUT_PULLUP);
  pinMode(buttonPin3, INPUT_PULLUP);

  debouncer.attach(buttonPin0);
  debouncer.interval(5); // Debounce interval in milliseconds
  debouncer.attach(buttonPin1);
  debouncer.interval(5); // Debounce interval in milliseconds
  debouncer.attach(buttonPin2);
  debouncer.interval(5); // Debounce interval in milliseconds
  debouncer.attach(buttonPin3);
  debouncer.interval(5); // Debounce interval in milliseconds    

  // BUTTONS TO TRIGGER THE STATE MACHINE STATES
  //STOP,
  //STARTING,
  //RUN_AUTO,
  //RUN_MANUAL
  attachInterrupt(digitalPinToInterrupt(buttonPin0), changeState0, FALLING);
  attachInterrupt(digitalPinToInterrupt(buttonPin1), changeState1, FALLING);
  attachInterrupt(digitalPinToInterrupt(buttonPin2), changeState2, FALLING);
  attachInterrupt(digitalPinToInterrupt(buttonPin3), changeState3, FALLING);
  

  // Initialize serial communication
  Serial.begin(9600);
  Serial.println("Setup complete");

  // Initialize PID controllers
  feederPID.SetMode(AUTOMATIC);
  blowerPID.SetMode(AUTOMATIC);

  // Initialize the display
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);

  // Clear the display
  display.clearDisplay();

  // Set the display contrast
  display.setTextColor(WHITE, BLACK);


}



// MAIN LOOP STARS HERE
void loop() {

  debouncer0.update();
  debouncer1.update();
  debouncer2.update();
  debouncer3.update();

  if (buttonPressed != -1) {
    handleButtonPress();
  }



  // FEEDER SCREW SPEED

      Feeder_on_Duration  = 1000;       // ON   time for Feeder_Relay   1 seconds
      Feeder_off_Duration = 20000;      // OFF  time for Feeder_Relay  20 seconds    
      Blower_on_Duration  = 1000;       // ON   time for Blower_Relay   1 seconds
      Blower_off_Duration = 3000;       // OFF  time for Blower_Relay   3 seconds


  // Read sensor values
  temperature = analogRead(temperaturePin) * 0.48828125;
  oxygenLevel = analogRead(oxygenPin) * 0.48828125;

  // Calculate PID outputs
  feederPID.Compute();
  blowerPID.Compute();




 currentMillis = millis();

 // Debugging feeder relay timing
//  Serial.print(" CurrentMillis: "); 
//  Serial.print(currentMillis);
//  Serial.print(" FeederRelayState: "); 
//  Serial.print(Feeder_Relay_State);
//  Serial.print(" RememberTime1: "); 
//  Serial.println(rememberTime1);

//////////////////////////////////////////////////////////////////////////////////////
// Set the Feeder Relay state, ON and OFF
//////////////////////////////////////////////////////////////////////////////////////

 if( Feeder_Relay_State == HIGH )
 {
    if( (currentMillis - rememberTime1) >= Feeder_on_Duration){   
    Feeder_Relay_State = LOW;                      // change the state of Feeder_Relay
    rememberTime1=currentMillis;                        // remember Current millis() time
    }
 }
 else
 {   
    if( (currentMillis - rememberTime1) >= Feeder_off_Duration){     
    Feeder_Relay_State = HIGH;                      // change the state of Feeder_Relay
    rememberTime1=currentMillis;                         // remember Current millis() time
    }
 }

 digitalWrite(Feeder_Relay_pin, Feeder_Relay_State);// turn the FEEDER RELAY ON or OFF
//////////////////////////////////////////////////////////////////////////////////////

//////////////////////////////////////////////////////////////////////////////////////
// Set the Blower Relay state, ON and OFF
//////////////////////////////////////////////////////////////////////////////////////

 if( Blower_Relay_State == HIGH )
 {
    if( (currentMillis - rememberTime2) >= Blower_on_Duration){   
    Blower_Relay_State = LOW;                      // change the state of Blower_Relay
    rememberTime2=currentMillis;                        // remember Current millis() time
    }
 }
 else
 {   
    if( (currentMillis - rememberTime2) >= Blower_off_Duration){     
    Blower_Relay_State = HIGH;                      // change the state of Blower_Relay
    rememberTime2=currentMillis;                         // remember Current millis() time
    }
 }

 digitalWrite(Blower_Relay_pin, Blower_Relay_State);// turn the Blower RELAY ON or OFF
//////////////////////////////////////////////////////////////////////////////////////

  // State-aware actions
  switch(state) {

    case States::STOP:

      displayInfo("STOP");

    break;


    case States::STARTING:

      displayInfo("STARTING");

    break;


    case States::RUN_AUTO:

      displayInfo("RUN AUTO");

    break;


    case States::RUN_MANUAL:

      displayInfo("RUN MANUAL");

    break;
  }

  // Simulate some delay and then switch to the next state
  delay(100);

}






void displayInfo(const char* stateName) {
  Serial.println(digitalRead(buttonPin));
  Serial.print(stateName);
  Serial.print(" | Temp: ");
  Serial.print(temperature);
  Serial.print(" | Oxygen: ");
  Serial.print(oxygenLevel);
  Serial.print(" | FRelay: ");
  Serial.print(digitalRead(Feeder_Relay_pin));
  Serial.print(" | BRelay: ");
  Serial.print(digitalRead(Blower_Relay_pin));
  Serial.println();

  display.clearDisplay();
  display.setCursor(0, 0);
  display.print("POLTTOMAATTI 1.3\n");
  display.print(stateName);
  display.print("\nLampotila: ");
  display.print(String(temperature, 1));
  display.print("\nHappiarvo: ");
  display.print(String(oxygenLevel, 1));
  display.print("\nRuuvi-Rele: ");
  display.print(digitalRead(Feeder_Relay_pin));
  display.print("\nIlma-Rele: ");
  display.print(digitalRead(Blower_Relay_pin));
  display.print("\n");
  display.display();
}