#include <Arduino.h>
// --- Hardware Pin Definitions ---
// Ultrasonic Sensor Pins
#define TRIG_A 5
#define ECHO_A 18
#define TRIG_B 19
#define ECHO_B 21

// Traffic Light Pins (Road A: West-East)
#define RED_A 23
#define YEL_A 22
#define GRN_A 32

// Traffic Light Pins (Road B: South-North)
#define RED_B 25
#define YEL_B 26
#define GRN_B 27

// --- Scale Setup ---
const float MAX_DISTANCE_D = 400.0; // cm
const float MIN_DISTANCE   = 5.0;   // cm

// Fuzzy Centroids (Green time range: 5s to 20s)
const float GREEN_SHORT  = 5.0;
const float GREEN_MEDIUM = 10.0;
const float GREEN_LONG   = 20.0;

// Triangular Membership Function
float trimf(float x, float a, float b, float c) {
  if (x <= a || x >= c) return 0.0;
  if (x == b) return 1.0;
  if (x > a && x < b) return (x - a) / (b - a);
  return (c - x) / (c - b);
}

// Distance Measurement with Cross-talk Settling Guard
float getDistanceCM(int trigPin, int echoPin) {
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  long duration = pulseIn(echoPin, HIGH, 30000);
  if (duration == 0) return MAX_DISTANCE_D;

  float distance = (duration * 0.0343) / 2.0;
  return constrain(distance, MIN_DISTANCE, MAX_DISTANCE_D);
}

// Fuzzy Congestion-to-Time Calculator for a Single Road
float calculateFuzzyGreenTime(float distance) {
  float congestion = ((MAX_DISTANCE_D - distance) / (MAX_DISTANCE_D - MIN_DISTANCE)) * 100.0;
  congestion = constrain(congestion, 0.0, 100.0);

  float mu_low  = trimf(congestion, -25.0, 0.0, 50.0);
  float mu_med  = trimf(congestion, 20.0, 50.0, 80.0);
  float mu_high = trimf(congestion, 50.0, 100.0, 125.0);

  if (congestion <= 0.0) mu_low = 1.0;
  if (congestion >= 100.0) mu_high = 1.0;

  float totalWeight = mu_low + mu_med + mu_high;
  if (totalWeight == 0.0) return GREEN_SHORT;

  return (mu_low * GREEN_SHORT + mu_med * GREEN_MEDIUM + mu_high * GREEN_LONG) / totalWeight;
}

void setLights(bool grnA, bool yelA, bool redA, bool grnB, bool yelB, bool redB) {
  digitalWrite(GRN_A, grnA); digitalWrite(YEL_A, yelA); digitalWrite(RED_A, redA);
  digitalWrite(GRN_B, grnB); digitalWrite(YEL_B, yelB); digitalWrite(RED_B, redB);
}

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

  pinMode(TRIG_A, OUTPUT); pinMode(ECHO_A, INPUT);
  pinMode(TRIG_B, OUTPUT); pinMode(ECHO_B, INPUT);

  pinMode(RED_A, OUTPUT); pinMode(YEL_A, OUTPUT); pinMode(GRN_A, OUTPUT);
  pinMode(RED_B, OUTPUT); pinMode(YEL_B, OUTPUT); pinMode(GRN_B, OUTPUT);

  setLights(false, false, true, false, false, true); // Initial All-Red
  Serial.println("2-Way Fuzzy Optimization System Online");
}

void loop() {
  // Read Road A distance
  float distA = getDistanceCM(TRIG_A, ECHO_A);
  delay(10); // Prevent ultrasonic pulse overlap between sensors
  float distB = getDistanceCM(TRIG_B, ECHO_B);

  // Compute individual fuzzy dynamic green times
  float timeA = calculateFuzzyGreenTime(distA);
  float timeB = calculateFuzzyGreenTime(distB);

  // Print Optimization Status
  Serial.println("\n------------------------------------------------");
  Serial.printf("ROAD A Dist: %.1f cm | Calculated Green: %.2f s\n", distA, timeA);
  Serial.printf("ROAD B Dist: %.1f cm | Calculated Green: %.2f s\n", distB, timeB);
  Serial.printf("OPTIMIZATION: Green Time Ratio (A:B) = %.2f : %.2f\n", timeA, timeB);
  Serial.println("------------------------------------------------");

  // --- PHASE 1: EXECUTE ROAD A GREEN ---
  setLights(true, false, false, false, false, true); // Road A Green, Road B Red
  delay((unsigned long)(timeA * 1000));

  // Yellow Transition Road A
  setLights(false, true, false, false, false, true);
  delay(2000);

  // --- PHASE 2: EXECUTE ROAD B GREEN ---
  // Re-read Road B distance right before green phase
  distB = getDistanceCM(TRIG_B, ECHO_B);
  timeB = calculateFuzzyGreenTime(distB);

  setLights(false, false, true, true, false, false); // Road A Red, Road B Green
  delay((unsigned long)(timeB * 1000));

  // Yellow Transition Road B
  setLights(false, false, true, false, true, false);
  delay(2000);
}