#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include "HX711.h"
Servo myservo;
LiquidCrystal_I2C lcd(0x27, 20, 4);
const int servoPin = 9; // Pin for the servo
const int buttonPin = 2; // The main button for servo control
const int heatingButtonPin = 3; // Button for the heating element (burner)
const int ledPin = 8; // Pin for the IRF520 gate
const int clearButtonPin = 4; // Button to clear the scale
int buttonState = LOW;
int lastButtonState = LOW;
int heatingButtonState = LOW;
int lastHeatingButtonState = LOW;
int servoPosition = 0;
int maxPosition = 80;
int startPosition = 20;
// HX711 setup
const int DOUT = 7;
const int SCK_HX711 = 6;
HX711 scale;

// Calibration factor for your load cell
float calibration_factor = -677.72;
// Variables for measurements
float lift;
bool heatingActive = false;
unsigned long heatingStartTime = 0;

bool running = false ; //Flag set when making measurements

void setup() {
  Serial.begin(115200);
  myservo.attach(servoPin);
  myservo.write(startPosition);
  pinMode(buttonPin, INPUT_PULLUP);
  pinMode(heatingButtonPin, INPUT_PULLUP); // Initialize the heating button
  pinMode(clearButtonPin, INPUT_PULLUP); // Initialize the clear button

  lcd.init();
  lcd.backlight();
  lcd.setCursor(0, 0);
  lcd.print("Angle Of Attack: "); // Initial text
  lcd.setCursor(0, 1);
  lcd.print("Lift: "); // Initial text
  lcd.setCursor(0, 2);
  lcd.print("Smoke: Ready "); // Initial text

  scale.begin(DOUT, SCK_HX711);
  scale.set_scale(calibration_factor); // Set the scale with your calibration factor
  scale.tare(); // Tare the scale
  pinMode(ledPin, OUTPUT);

  digitalWrite(ledPin, LOW); // Ensure the IRF520 is initially off
}
void loop() {
  unsigned long currentMillis = millis();

  int heatingButtonReading = digitalRead(heatingButtonPin);
  checkbuttonReading();
  checkClearScale();

  doServo();

  // Heating element (burner) control
  if (heatingButtonReading == LOW && !heatingActive) {
    heatingActive = true;
    heatingStartTime = currentMillis;
    digitalWrite(ledPin, HIGH);
    lcd.setCursor(0, 2);
    lcd.print("Smoke: Active ");
  }
  if (heatingActive) {
    if (currentMillis - heatingStartTime >= 10000) {
      heatingActive = false;
      digitalWrite(ledPin, LOW);
      lcd.setCursor(0, 2);
      lcd.print("Smoke: Cooldown");
    } else if (currentMillis - heatingStartTime >= 5000) {
      lcd.setCursor(0, 2);
      lcd.print("Smoke: Ready ");
    }
  }

  lastHeatingButtonState = heatingButtonReading;

} // End loop

float GetWeight() {

  // Take a single weight measurement
  lift = scale.get_units(10);
  return lift;
}

void checkClearScale() {
  // Clear the scale if the clear button is pressed
  int clearButtonReading = digitalRead(clearButtonPin);
  if (clearButtonReading == LOW) {
    scale.tare(); // Clear the scale
    lift = 0.00; // Set the lift value to 0.00
    lcd.setCursor(6, 1);
    lcd.print("0.00  "); // Display "0.00" as the cleared weight
    running = false; //Stop run 
  }
}

void checkbuttonReading() {
  //Check if start button pressed
  static bool lastButtonState = HIGH;
  bool buttonReading = digitalRead(buttonPin);
  if (buttonReading != lastButtonState ) {
    if (buttonReading == LOW) {
      running = true;
      Serial.println("Running.");
    } else {
      Serial.println("Button release");

    }

    lastButtonState = buttonReading;
  }

}
  void doServo() {
#define STEPDELAY 1000 //Delay in mSecs between steps
#define STEPSIZE 5 // Servo step size 
#define SERVOSETTLE 200 //Time for servo position to stabilise before weight reading

    static unsigned long nextActionTime = 0;
    static int pos = startPosition, lastPos = -1;
    static bool SettleWait = false;

    unsigned long now = millis();

    if (!running) return; //Halted
    if (now  <  nextActionTime) return; //Inbetween - not doing anything
    if ( !SettleWait) {
      //Time for another step
      SettleWait = true ;
      nextActionTime = now + SERVOSETTLE;

      //Make a step
      if (lastPos < 0 ) {
        pos = startPosition;
      } else {
        pos += STEPSIZE ;
      }

      //Check for limit on position
      if (pos > maxPosition) {
        //End of run
        myservo.write(startPosition);
        servoPosition = 0;
        lcd.setCursor(17, 0);
        lcd.print("0  ");

        running = false;
        lastPos = -1;
        return;
      }

      lastPos = pos; //Note current position
      // Move the servo to the desired position
      myservo.write(pos);
      servoPosition = pos - startPosition;
      lcd.setCursor(17, 0);
      lcd.print(servoPosition);
      Serial.print(" Angle Of Attack: ");
      Serial.println(servoPosition);

    } else {
      //Time to get weight
      SettleWait  = false;
      nextActionTime = now + STEPDELAY ;
      lift = GetWeight();
      lcd.setCursor(0, 1);
      lcd.print("Lift: ");
      lcd.print(lift, 2);
      Serial.print(" Lift: ");
      Serial.println(lift, 2);

    }

  }