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


// Arduino Mega interrupt pins:
// INT.0 on Pin 21
// INT.1 on Pin 20
// INT.2 on Pin 19
// INT.3 on Pin 18
// INT.4 on Pin 2
// INT.5 on Pin 3


const byte buttonPin0 = 2;     // Interrupt pins  
const byte buttonPin1 = 3;
const byte buttonPin2 = 18;
const byte buttonPin3 = 19;

enum States { STOP, STARTING, AUTO, USER };
volatile States state = STOP;

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

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

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;

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

const int temperaturePin = A0;
const int oxygenPin = A1;

const int Feeder_Relay_pin = 7;
unsigned long Feeder_on_Duration = 2000;
unsigned long Feeder_off_Duration = 20000;
unsigned long rememberTime1 = 0;
int Feeder_Relay_State = LOW;

const int Blower_Relay_pin = 8;
unsigned long Blower_on_Duration = 2000;
unsigned long Blower_off_Duration = 2000;
unsigned long rememberTime2 = 0;
int Blower_Relay_State = LOW;

#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire);

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 = AUTO;
  if (buttonPressed == 3) state = USER;
  buttonPressed = -1;
  Serial.print("STATE: ");
  Serial.println(state);
}

void displayInfo(const char* stateName);

void setup() {
  Serial.begin(9600);

  pinMode(buttonPin0, INPUT_PULLUP);
  pinMode(buttonPin1, INPUT_PULLUP);
  pinMode(buttonPin2, INPUT_PULLUP);
  pinMode(buttonPin3, INPUT_PULLUP);

  debouncer0.attach(buttonPin0);
  debouncer0.interval(5);
  debouncer1.attach(buttonPin1);
  debouncer1.interval(5);
  debouncer2.attach(buttonPin2);
  debouncer2.interval(5);
  debouncer3.attach(buttonPin3);
  debouncer3.interval(5);

  attachInterrupt(digitalPinToInterrupt(buttonPin0), changeState0, FALLING);
  attachInterrupt(digitalPinToInterrupt(buttonPin1), changeState1, FALLING);
  attachInterrupt(digitalPinToInterrupt(buttonPin2), changeState2, FALLING);
  attachInterrupt(digitalPinToInterrupt(buttonPin3), changeState3, FALLING);

  //attachInterrupt(digitalPinToInterrupt(buttonPin0), changeState0, FALLING);
  //attachInterrupt(digitalPinToInterrupt(buttonPin1), changeState1, FALLING);
  //attachInterrupt(digitalPinToInterrupt(buttonPin2), changeState2, RISING);  // Changed from FALLING to RISING
  //attachInterrupt(digitalPinToInterrupt(buttonPin3), changeState3, RISING);  // Changed from FALLING to RISING


  pinMode(Feeder_Relay_pin, OUTPUT);
  pinMode(Blower_Relay_pin, OUTPUT);

  feederPID.SetMode(AUTOMATIC);
  blowerPID.SetMode(AUTOMATIC);

  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
  display.clearDisplay();
  display.setTextColor(WHITE, BLACK);

  Serial.println("Setup complete");
}

void loop() {
  debouncer0.update();
  debouncer1.update();
  debouncer2.update();
  debouncer3.update();

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


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

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

  //////////////////////////////////////////////////////////////////////////////////
  // FEEDER AND BLOWER RELAY TIMER,  counting millisecond ticks
  //////////////////////////////////////////////////////////////////////////////////
        unsigned long currentMillis = millis();

        // Set the Feeder Relay state, ON and OFF
        if (Feeder_Relay_State == HIGH) {
          if (currentMillis - rememberTime1 >= Feeder_on_Duration) {
            Feeder_Relay_State = LOW;
            rememberTime1 = currentMillis;
            Serial.println("Feeder Relay OFF");
          }
        } else {
          if (currentMillis - rememberTime1 >= Feeder_off_Duration) {
            Feeder_Relay_State = HIGH;
            rememberTime1 = currentMillis;
            Serial.println("Feeder Relay ON");
          }
        }


        // Set the Blower Relay state, ON and OFF
        if (Blower_Relay_State == HIGH) {
          if (currentMillis - rememberTime2 >= Blower_on_Duration) {
            Blower_Relay_State = LOW;
            rememberTime2 = currentMillis;
            Serial.println("Blower Relay OFF");
          }
        } else {
          if (currentMillis - rememberTime2 >= Blower_off_Duration) {
            Blower_Relay_State = HIGH;
            rememberTime2 = currentMillis;
            Serial.println("Blower Relay ON");
          }
        }

  //////////////////////////////////////////////////////////////////////////////////



  switch (state) {
    case STOP:
              displayInfo("STOP");
              digitalWrite(Feeder_Relay_pin, false);
              digitalWrite(Blower_Relay_pin, false);                
    break;


    case STARTING:
              displayInfo("STARTING");
              digitalWrite(Feeder_Relay_pin, Feeder_Relay_State);
              digitalWrite(Blower_Relay_pin, Blower_Relay_State);
              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              
    break;


    case AUTO:
              displayInfo("RUN AUTO");
              digitalWrite(Feeder_Relay_pin, Feeder_Relay_State);
              digitalWrite(Blower_Relay_pin, Blower_Relay_State);
              Feeder_on_Duration  = 500;       // TAHAN PID-SAATIMELTA LASKENNALLINEN ARVO, taytyy kehitella laskukaava..
              Feeder_off_Duration = 5000;      // TAHAN PID-SAATIMELTA LASKENNALLINEN ARVO, taytyy kehitella laskukaava..  
              Blower_on_Duration  = 500;       // TAHAN PID-SAATIMELTA LASKENNALLINEN ARVO, taytyy kehitella laskukaava..
              Blower_off_Duration = 2000;      // TAHAN PID-SAATIMELTA LASKENNALLINEN ARVO, taytyy kehitella laskukaava..
    break;


    case USER:
              displayInfo("RUN MANUAL");
              digitalWrite(Feeder_Relay_pin, Feeder_Relay_State);
              digitalWrite(Blower_Relay_pin, Blower_Relay_State);
              Feeder_on_Duration  = 1000;       // TAHAN KAYTTAJAN OMA ASETUS, SAATO VALIKOSTA TAI ETHERNETIN KAUTTA..
              Feeder_off_Duration = 20000;      // TAHAN KAYTTAJAN OMA ASETUS, SAATO VALIKOSTA TAI ETHERNETIN KAUTTA..
              Blower_on_Duration  = 1000;       // TAHAN KAYTTAJAN OMA ASETUS, SAATO VALIKOSTA TAI ETHERNETIN KAUTTA..
              Blower_off_Duration = 3000;       // TAHAN KAYTTAJAN OMA ASETUS, SAATO VALIKOSTA TAI ETHERNETIN KAUTTA.. 
    break;
  }
}

void displayInfo(const char* stateName) {
  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.5\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();
}