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

#include <PID_v1.h>

// 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 sensor and relay pins
const int temperaturePin = A0;
const int oxygenPin = A1;
const int feederRelay = 2;
const int blowerRelay = 3;
const int timerFeederPin = 4; // pin to control the feeder

// Define variables
double temperature;
double desiredTemperature = 100.0;
double feederSpeed;
double blowerSpeed;
double oxygenLevel;
double targetOxygenLevel = 21.0;
bool isRunning = true;
bool manualMode = false;
int timerFeederValue; // 8-bit control value for the feeder


//Manual mode, run the Feeder with constant values..
//             like 2 seconds, everyy 20 seconds..
int feederOnDuration = 2000; // Feed duration in milliseconds (2 seconds)
int feederPauseDuration = 20000; // Pause duration in milliseconds (20 seconds)

unsigned long lastFeederOnTime = 0;
unsigned long currentFeedertime = 0;


const int LEDpin = 7;
const long onDuration = 2000;// OFF time for LED
const long offDuration = 20000;// ON time for LED

long rememberTime=0;// this is used by the code
int LEDState =HIGH;// initial state of LED

// 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 the display parameters
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64

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

void setup() {

  // Initialize serial communication
  Serial.begin(9600);

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

  // Set relay pins as outputs
  pinMode(feederRelay, OUTPUT);
  pinMode(blowerRelay, OUTPUT);
  pinMode(timerFeederPin, OUTPUT);


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

  // Clear the display
  display.clearDisplay();

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

void loop() {



 if( LEDState ==HIGH )
 {
    if( (millis()- rememberTime) >= onDuration){   
    LEDState = LOW;// change the state of LED
    rememberTime=millis();// remember Current millis() time
    }
 }
 else
 {   
    if( (millis()- rememberTime) >= offDuration){     
    LEDState =HIGH;// change the state of LED
    rememberTime=millis();// remember Current millis() time
    }
 }

 // Robojax LED blink with millis()
 digitalWrite(LEDpin,LEDState);// turn the LED ON or OFF




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

  // Calculate PID outputs
  if (!manualMode) {
    feederPID.Compute();
    blowerPID.Compute();
  }


  // FEEDER SCREW SPEED
  if (manualMode == true ){

      // Check if it's time to feed or pause
      unsigned long currentTime = millis();
      if (currentTime - lastFeederOnTime >= feederOnDuration) {
        // It's been enough time to feed, turn on the relay
        digitalWrite(feederRelay, HIGH);
        currentFeedertime = currentTime;
        lastFeederOnTime = currentTime + feederPauseDuration;
      } else if (currentTime - currentFeedertime >= feederPauseDuration) {
        // It's been enough time to pause, turn off the relay
        digitalWrite(feederRelay, LOW);
        currentFeedertime = 0; // Reset feedertime
        lastFeederOnTime = 0; // Reset last feedertime
      }
  }

  if (manualMode == false ){
      digitalWrite(feederRelay, feederSpeed);
  }

  //AIR BLOWER SPEED 
  digitalWrite(blowerRelay, blowerSpeed);

  // Control the feeder using an 8-bit control value
  timerFeederValue = map(feederSpeed, -100, 100, 0, 255);
  analogWrite(timerFeederPin, timerFeederValue);

  // Check for MODE SELECT SWITCH
  if (digitalRead(22) == HIGH) {
    manualMode= true;
  }
  else {
    manualMode= false;
  }


  if (manualMode == false) {
    Serial.print("AUTOMODE | ");
    Serial.print("Temp: ");
    Serial.print(temperature);
    Serial.print(" | Oxygen: ");
    Serial.print(oxygenLevel);
    Serial.print(" | Feeder Speed: ");
    Serial.print(feederSpeed);
    Serial.print(" | Blower Speed: ");
    Serial.println(blowerSpeed);  
  }
  if (manualMode == true) {
    Serial.print("MANUALMODE | ");
    Serial.print("Temp: ");
    Serial.print(temperature);
    Serial.print(" | Oxygen: ");
    Serial.print(oxygenLevel);
    Serial.print(" | FRelay: ");

  if (LEDState == HIGH) {     
     Serial.print("1");
  } 
  else {
     Serial.print("0");    
  }

    Serial.print(" | BRelay: ");

  if (blowerRelay == HIGH) {     
     Serial.print("1");
  } 
  else {
     Serial.print("0");    
  }


//    Serial.print(" | Cur. Feeder time: ");
//    Serial.print(currentFeedertime);
    Serial.print(" | Last FeedT.: ");
    Serial.println(lastFeederOnTime );  
  }
  // Send data to serial monitor

  

  // Display LCD SCREEN INFORMATION FOR HUMAN OPERATOR
  display.setCursor(0, 0);
  display.print("POLTTOMAATTI 1.1      \n");
  display.print("Lampotila: ");
  display.print(String(temperature,1));
  display.print("\n");
  display.print("Happiarvo: ");
  display.print(String(oxygenLevel,1)); 
  display.print("\n");  
  display.print("Ruuvi-Rele: ");
  display.print(digitalRead(feederRelay));
  display.print("\n");
  display.print("Ilma-Rele: ");
  display.print(digitalRead(blowerRelay));
  display.print("\n");

  if (!manualMode) {
    display.setCursor(0, 1);
    display.print("Manual Mode: ");
    display.print(manualMode);
    display.print("\n");
    display.print("Feeder Timer Value: ");
    display.println(String(timerFeederValue));
  }

  // Display the text
  display.display();

  // Delay between readings
  delay(500);

}