#include <LiquidCrystal_I2C.h>
#include <HX711.h>
#include <Servo.h>
#include "PID_v1.h"

// define konstanta
#define DATA_OUT 4
#define SCK_OUT 3

#define SERVO_PIN 10
#define SETPOINT_PIN A0
#define CONFIRM_PIN 12
#define START_PIN 11

// Initiate LCD I2C
LiquidCrystal_I2C lcd(0x27, 16, 2);

// Initiate HX711 Scale
HX711 scale;
float calibrationFactor = 420;
float units = 0;

// Variable PID
double setpoint, input, output;
double Kp = 180, Ki = 0.003, Kd = 0; // PI

// Initiate Servo & PID
Servo srv;
PID pidcs(&input, &output, &setpoint, Kp, Ki, Kd, P_ON_E, DIRECT);

// Aux
float sudut = 0.0;
boolean isConfirm = false;
boolean isStart = false;


// Function ini adalah kebutuhan simulasi pengisian
// Seolah-olah dilakukan pengisian tanpa harus 
// menggerakan sensor secara manual
float simulate(float factor, boolean isStart) {
  static uint32_t last = 0;
  static float result = 0;
  const int interval = 50; // interval proses print

  float area = 0.000314 * factor; //m2
  float debitM3 = area * 4.42718872424; // m3/s 4.42718872424 = sqrt(2 * g * 1)
  float debitLiter = debitM3 * 1000; // liter/s
  float ro = 0.74; // kg/liter
  float weightPerSecond = debitLiter * ro; // kg/s
  
  if (millis() - last > interval) {
    last += interval;
    result = result + (weightPerSecond/interval);

    if (!isStart) {
      result = 0.0;
    }
  }

  return result;
}

// Ini adalah function untuk memberikan mapping interpolasi
// dengan nilai keluaran bertipe float
// sehingga ada nilai koma yang menunjukan
// ke presisian dari sistem
float linearInterpolation(float x, float x0, float x1, float y0, float y1) {
  // Check if x is within the range [x0, x1]
  if (x < x0) {
    return y0;
  } else if (x > x1) {
    return y1;
  }
  
  // Perform linear interpolation
  float y = y0 + (x - x0) * (y1 - y0) / (x1 - x0);
  return y;
}

void setup() {

  Serial.begin(115200);

  // Tare should be confirmed
  pinMode(CONFIRM_PIN, INPUT_PULLUP);
  pinMode(START_PIN, INPUT_PULLUP);

  // Set LCD
  lcd.begin(16,2);
  lcd.init();
  lcd.backlight();

  // Set Servo
  srv.attach(SERVO_PIN);
  srv.write(180);

  // Set HX711
  scale.begin(DATA_OUT, SCK_OUT);
  scale.set_scale(calibrationFactor);

  // Konfirmasi Tare 
  //
  // Problem:
  // Wokwi membaca tare setiap saat dalam simulasinya
  // https://docs.wokwi.com/parts/wokwi-hx711
  // Jadi pastikan scale pada kondisi zero
  // Proses akan ditahan selama belum terkonfirmasi oleh user

  // Solution:
  // dibuatkan proses penungguan untuk adjust scale pada load cell
  // Ketika tombol hijau ditekan, akan dikonfirmasi untuk dilanjutkan
  // Intinya, pastikan tare scale harus 0 
  while(!isConfirm) {
    lcd.setCursor(0,0);
    lcd.print("Tare: Adjust scale");
    lcd.setCursor(0,1);
    lcd.print(scale.get_units(), 1);

    if (digitalRead(CONFIRM_PIN) == LOW) {
      isConfirm = true;
    }
  }

  lcd.clear();
  lcd.setCursor(0,0);
  lcd.print("Tare Confirmed");
  scale.tare();

  // inisiasi input
  input = scale.get_units(), 5;
  setpoint = map(analogRead(SETPOINT_PIN), 0, 1023, 5, 1);

  // define limit of pid
  pidcs.SetOutputLimits(-4, 180);

  // Nyalakan PID
  pidcs.SetMode(AUTOMATIC);

  lcd.setCursor(0,0);
  lcd.print("Filling Machine");

  delay(2000);
  lcd.clear();
}

void display() {
  static uint32_t last = 0;
  const int interval = 5; // interval proses print

  if (millis() - last > interval) {
    last += interval;

    Serial.print("SP:");
    Serial.print(linearInterpolation(setpoint, 1.0, 5.0, 0.0, 180.0));
    Serial.print(" PV:");
    Serial.print(linearInterpolation(input, 1.0, 5.0, 0.0, 180.0));
    Serial.print(" CV:");
    Serial.print(sudut);
    Serial.print(' ');
    Serial.println();

    if (isStart) {
      lcd.clear();
      lcd.setCursor(0,0);
      lcd.print("SP:");
      lcd.setCursor(4,0);
      lcd.print(setpoint);
      lcd.setCursor(0,1);
      lcd.print("KG:");
      lcd.setCursor(4,1);
      lcd.print(input);
    } else {
      lcd.setCursor(0,0);
      lcd.print("Status: Stopped");
      lcd.setCursor(0,1);
      lcd.print("Press Start");
    }
  }

}


void loop() {

  // Real input sensor
  // input = scale.get_units();

  // ini bagian simulasi
  // Hilangkan tanda komentar (//) untuk memulai simulasi
  if (isStart) { // <-- beri komentar dari sini
    float _factor = linearInterpolation(output, 0.0, 180.0, 0.0, 0.98);
    float _sample = simulate(_factor, true);
    input = _sample;
  } else {
    input = simulate(0, false);
  }// <-- hingga sini untuk menghilangkan simulasi

  // Waiting for trigger start
  if (digitalRead(START_PIN) == LOW) {
    isStart = true;
  }

  if (pidcs.Compute() && isStart) {
    srv.write(sudut);
    sudut = linearInterpolation(output, 0.0, 180.0, 180.0, 0.0);
    setpoint = linearInterpolation(analogRead(SETPOINT_PIN), 0.0, 1023.0, 5.0, 1.0);
  }

  // Stop jika valve sudah full close
  if (output < 0.5) {
    isStart = false;
  }

  display();
}