#include <Wire.h>
#include <LiquidCrystal_I2C.h>
#include <Servo.h>
#include <math.h>

#define NUM_FIELDS 4
#define SENSORS_PER_FIELD 4
#define BASE_SERVO_TIME 4000
#define TOTAL_SERVO_LOOP_TIME 24000
#define EXTRA_TIME 8000

const int potPins[NUM_FIELDS][SENSORS_PER_FIELD] = {
  {A0, A1, A2, A3},
  {A4, A5, A6, A7},
  {A8, A9, A10, A11},
  {A12, A13, A14, A15}
};

const int muxControlPins[4] = {6, 7, 8, 9};
const int triggerPin = 10;
const int echoPin = 11;

// I2C LCD display addresses
#define I2C_ADDR1 0x27
#define I2C_ADDR2 0x20

// I2C LCD displays
LiquidCrystal_I2C lcd1(I2C_ADDR1, 20, 4);
LiquidCrystal_I2C lcd2(I2C_ADDR2, 20, 4);

Servo servos[NUM_FIELDS];
const int servoPins[NUM_FIELDS] = {2, 3, 4, 5};

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

  // Initialize LCDs
  lcd1.init();
  lcd1.backlight();
  lcd2.init();
  lcd2.backlight();

  // Initialize servos
  for (int i = 0; i < NUM_FIELDS; i++) {
    servos[i].attach(servoPins[i]);
    servos[i].write(0);
  }

  // Initialize pins
  pinMode(triggerPin, OUTPUT);
  pinMode(echoPin, INPUT);
  for (int i = 0; i < 4; i++) {
    pinMode(muxControlPins[i], OUTPUT);
  }

  // Display startup message on LCD1
  lcd1.setCursor(2, 0);
  lcd1.print("Auto-Irrigation");
  lcd1.setCursor(7, 1);
  lcd1.print("System");
  lcd1.setCursor(9, 2);
  lcd1.print("by");
  lcd1.setCursor(7, 3);
  lcd1.print("Team 4");
  lcd2.setCursor(2, 0);
  lcd2.print("Total Overview");
  lcd2.setCursor(7, 1);
  lcd2.print("System");
  lcd2.setCursor(7, 3);
  lcd2.print("Ready");
  delay(2000);
  lcd2.clear();
}

void loop() {
  float moistureLevels[NUM_FIELDS];
  float totalMoisture = 0;
  float waterDischarge[NUM_FIELDS];
  float totalWaterVolume = 0;
  int totalLoopTime = 0;

  // Calculate field-specific data
  for (int field = 0; field < NUM_FIELDS; field++) {
    float fieldMoistureSum = 0;

    // Read and average sensor data for the field
    for (int sensor = 0; sensor < SENSORS_PER_FIELD; sensor++) {
      int potValue = analogRead(potPins[field][sensor]);
      fieldMoistureSum += map(potValue, 0, 1023, 0, 100);
    }

    moistureLevels[field] = fieldMoistureSum / SENSORS_PER_FIELD;
    totalMoisture += (100 - moistureLevels[field]);

    // Measure distance for water discharge calculation
    selectMuxChannel(field);
    float distance = measureDistance();
    float h_meters = distance / 100.0;

    // Calculate water discharge rate
    waterDischarge[field] = (0.3 + (tan(radians(15)) * (0.5 - h_meters))) * (0.5 - h_meters);

    displayDataOnLCD1(field, moistureLevels[field], waterDischarge[field]);
  }

  // Activate each field's servo and calculate total time and volume
  for (int field = 0; field < NUM_FIELDS; field++) {
    int additionalTime = (totalMoisture > 0) ? (int)(EXTRA_TIME * ((100 - moistureLevels[field]) / totalMoisture)) : 0;
    int fieldTime = BASE_SERVO_TIME + additionalTime;

    servos[field].write(90);
    delay(fieldTime);
    servos[field].write(0);

    // Calculate water volume
    float fieldVolume = waterDischarge[field] * (fieldTime / 1000.0);
    totalWaterVolume += fieldVolume;
    totalLoopTime += fieldTime;

    Serial.print("Field ");
    Serial.print(field + 1);
    Serial.print(" Avg Moisture: ");
    Serial.print(moistureLevels[field]);
    Serial.print("%, Q: ");
    Serial.print(waterDischarge[field], 6);
    Serial.print(" m^3/s, Volume: ");
    Serial.print(fieldVolume, 3);
    Serial.print(" m^3, Time: ");
    Serial.print(fieldTime / 1000.0, 1);
    Serial.println(" s");

    delay(500);
  }

  // Display total data on the second LCD
  displayTotalDataOnLCD2(totalLoopTime / 1000.0, totalWaterVolume);
  delay(1000);
}

void selectMuxChannel(int channel) {
  for (int i = 0; i < 4; i++) {
    digitalWrite(muxControlPins[i], (channel & (1 << i)) ? HIGH : LOW);
  }
}

float measureDistance() {
  digitalWrite(triggerPin, LOW);
  delayMicroseconds(2);
  digitalWrite(triggerPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(triggerPin, LOW);

  long duration = pulseIn(echoPin, HIGH);
  float distance = (duration * 0.0343) / 2;
  return distance;
}

void displayDataOnLCD1(int fieldIndex, float moisture, float discharge) {
  lcd1.clear();
  lcd1.setCursor(0, 0);
  lcd1.print("Field ");
  lcd1.print(fieldIndex + 1);
  lcd1.print(" Mstr:");
  lcd1.setCursor(0, 1);
  lcd1.print(moisture, 1);
  lcd1.print("%");
  lcd1.setCursor(0, 2);
  lcd1.print("Q: ");
  lcd1.print(discharge, 6);
  lcd1.print(" m3/s");
  delay(2000);
}

void displayTotalDataOnLCD2(float totalTime, float totalVolume) {
  lcd2.clear();
  lcd2.setCursor(0, 0);
  lcd2.print("Total Time:");
  lcd2.setCursor(0, 1);
  lcd2.print(totalTime, 1);
  lcd2.print(" s");
  lcd2.setCursor(0, 2);
  lcd2.print("Total Volume:");
  lcd2.setCursor(0, 3);
  lcd2.print(totalVolume, 3);
  lcd2.print(" m^3");
}