// 4x Distance Control v4.2
// by Myth!
 
// This code performs continuous distance measurement from four sensors,
// activates proximity warnings, alarms, and displays status messages.
// It utilizes LED indicators, an OLED display, and audible alerts for comprehensive monitoring.

#include "ssd1306.h"
#include <FastLED_NeoPixel.h>

// Pin&Param definitions
#define DATA_PIN 14
#define NUM_LEDS 8
#define BRIGHTNESS 255
#define PIN_TRIG_1 3
#define PIN_ECHO_1 2
#define PIN_TRIG_2 5
#define PIN_ECHO_2 4
#define PIN_TRIG_3 9
#define PIN_ECHO_3 8
#define PIN_TRIG_4 7
#define PIN_ECHO_4 6
#define SPEAKER_PIN 13
#define RED_LED_PIN 12
#define YELLOW_LED_PIN 11
#define GREEN_LED_PIN 10

// Distance thresholds (cm)
const int Warning_Dist = 80;
const int Alarm_Dist = 30;

FastLED_NeoPixel<NUM_LEDS, DATA_PIN, NEO_GRB> strip;

// Function prototypes
void initializeHardware();
int measureDistance(int trigPin, int echoPin);
void printDistance(int yPos, int duration, const char* sensorName, bool bold);
bool checkProximity(int duration_1, int duration_2, int duration_3, int duration_4, int minDistance, int maxDistance);
bool checkAlarm(int duration_1, int duration_2, int duration_3, int duration_4, int alarmDistance);
void activateWarning(int duration_1, int duration_2, int duration_3, int duration_4);
void activateAlarm(int duration_1, int duration_2, int duration_3, int duration_4);
void clearNotification(bool isWarning);
void activateOK();
void clearOK();

void setup() {
  initializeHardware(); // Initialize hardware components
  
  // Display initialization message
  // This is not necessary, but can be useful in Wokwi to reposition sensors :)
  ssd1306_clearScreen();
  ssd1306_printFixed(14, 0, "DISTANCE CONTROL", STYLE_BOLD);
  ssd1306_printFixed(40, 16, "by Myth!", STYLE_ITALIC);
  delay(2000);
  ssd1306_printFixed(24, 48, "Calibration...", STYLE_ITALIC);
  delay(5000);
  ssd1306_clearScreen();
  ssd1306_printFixed(14, 0, "DISTANCE CONTROL", STYLE_BOLD);
}

void loop() {
  // Measure distances from sensors
  int duration_1 = measureDistance(PIN_TRIG_1, PIN_ECHO_1);
  int duration_2 = measureDistance(PIN_TRIG_2, PIN_ECHO_2);
  int duration_3 = measureDistance(PIN_TRIG_3, PIN_ECHO_3);
  int duration_4 = measureDistance(PIN_TRIG_4, PIN_ECHO_4);
  
  // Print distances to OLED display
  printDistance(16, duration_1, " Sensor 1", duration_1 <= Warning_Dist);
  printDistance(24, duration_2, " Sensor 2", duration_2 <= Warning_Dist);
  printDistance(32, duration_3, " Sensor 3", duration_3 <= Warning_Dist);
  printDistance(40, duration_4, " Sensor 4", duration_4 <= Warning_Dist);
  
  // Check proximity and activate warnings if necessary
  if (checkProximity(duration_1, duration_2, duration_3, duration_4, Alarm_Dist, Warning_Dist)) {
    clearOK();
    activateWarning(duration_1, duration_2, duration_3, duration_4);
  } else {
    clearNotification(true);
    if (!checkAlarm(duration_1, duration_2, duration_3, duration_4, Alarm_Dist)) {
      activateOK();
    }
  }
  
  // Check for alarm conditions and activate alarm if necessary
  if (checkAlarm(duration_1, duration_2, duration_3, duration_4, Alarm_Dist)) {
    clearOK();
    activateAlarm(duration_1, duration_2, duration_3, duration_4);
  } else {
    clearNotification(false);
    if (!checkProximity(duration_1, duration_2, duration_3, duration_4, Alarm_Dist, Warning_Dist)) {
      activateOK();
    }
  }
}

// Initialize hardware components
void initializeHardware() {
  // Initialize LED strip
  strip.begin();
  strip.setBrightness(BRIGHTNESS);
  strip.setPixelColor(6, strip.Color(0, 0, 255)); 
  strip.setPixelColor(2, strip.Color(0, 0, 255)); 
  strip.show();
  
  // Set pin modes for ultrasonic sensors
  pinMode(PIN_TRIG_1, OUTPUT);
  pinMode(PIN_ECHO_1, INPUT);
  pinMode(PIN_TRIG_2, OUTPUT);
  pinMode(PIN_ECHO_2, INPUT);
  pinMode(PIN_TRIG_3, OUTPUT);
  pinMode(PIN_ECHO_3, INPUT);
  pinMode(PIN_TRIG_4, OUTPUT);
  pinMode(PIN_ECHO_4, INPUT);
  
  // Set pin modes for output devices
  pinMode(SPEAKER_PIN, OUTPUT);
  pinMode(RED_LED_PIN, OUTPUT);
  pinMode(YELLOW_LED_PIN, OUTPUT);
  pinMode(GREEN_LED_PIN, OUTPUT);
  
  // Initialize OLED display
  ssd1306_128x64_i2c_init();
  ssd1306_setFixedFont(ssd1306xled_font6x8);
}

// Measure distance using ultrasonic sensor
int measureDistance(int trigPin, int echoPin) {
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  return pulseIn(echoPin, HIGH) / 58; // Distance in cm
}

// Print distance to OLED display
void printDistance(int yPos, int duration, const char* sensorName, bool bold) {
  String distanceString = String(sensorName) + ": " + String(duration) + " cm  ";
  ssd1306_printFixed(8, yPos, distanceString.c_str(), bold ? STYLE_BOLD : STYLE_NORMAL);
}

// Check if proximity warning should be activated
bool checkProximity(int duration_1, int duration_2, int duration_3, int duration_4, int minDistance, int maxDistance) {
  return ((duration_1 < maxDistance && duration_1 > minDistance) ||
          (duration_2 < maxDistance && duration_2 > minDistance) ||
          (duration_3 < maxDistance && duration_3 > minDistance) ||
          (duration_4 < maxDistance && duration_4 > minDistance));
}

// Check if alarm condition is met
bool checkAlarm(int duration_1, int duration_2, int duration_3, int duration_4, int alarmDistance) {
  return (duration_1 < alarmDistance) || 
         (duration_2 < alarmDistance) || 
         (duration_3 < alarmDistance) || 
         (duration_4 < alarmDistance);
}

// Activate proximity warning
void activateWarning(int duration_1, int duration_2, int duration_3, int duration_4) {
  digitalWrite(YELLOW_LED_PIN, HIGH);
  strip.setPixelColor(1, (duration_1 < Warning_Dist && duration_1 > Alarm_Dist) ? strip.Color(255, 255, 0) : 0);
  strip.setPixelColor(3, (duration_2 < Warning_Dist && duration_2 > Alarm_Dist) ? strip.Color(255, 255, 0) : 0);
  strip.setPixelColor(5, (duration_3 < Warning_Dist && duration_3 > Alarm_Dist) ? strip.Color(255, 255, 0) : 0);
  strip.setPixelColor(7, (duration_4 < Warning_Dist && duration_4 > Alarm_Dist) ? strip.Color(255, 255, 0) : 0);
  strip.show();
  ssd1306_printFixed(28, 56, "   WARNING   ", STYLE_BOLD);
  tone(SPEAKER_PIN, 1500);
  delay(200);
  noTone(SPEAKER_PIN);
  int min_duration = min(min(min(duration_1, duration_2), duration_3), duration_4);
  delay(min_duration * 10);
}

// Activate alarm
void activateAlarm(int duration_1, int duration_2, int duration_3, int duration_4) {
  digitalWrite(RED_LED_PIN, HIGH);
  strip.setPixelColor(1, (duration_1 < Alarm_Dist) ? strip.Color(255, 0, 0) : 0);
  strip.setPixelColor(3, (duration_2 < Alarm_Dist) ? strip.Color(255, 0, 0) : 0);
  strip.setPixelColor(5, (duration_3 < Alarm_Dist) ? strip.Color(255, 0, 0) : 0);
  strip.setPixelColor(7, (duration_4 < Alarm_Dist) ? strip.Color(255, 0, 0) : 0);
  strip.show();
  ssd1306_negativeMode();
  ssd1306_printFixed(28, 56, "    ALARM    ", STYLE_BOLD);
  ssd1306_positiveMode();
  tone(SPEAKER_PIN, 1520);
  delay(200);
  noTone(SPEAKER_PIN);
  int min_duration = min(min(min(duration_1, duration_2), duration_3), duration_4);
  delay(min_duration * 10);
}

// Clear warning or alarm
void clearNotification(bool isWarning) {
  ssd1306_printFixed(28, 56, "               ", STYLE_BOLD);
  digitalWrite(isWarning ? YELLOW_LED_PIN : RED_LED_PIN, LOW);
  strip.clear();
  strip.show();
}

// Activate OK state, adjust measurement frequency
void activateOK() {
  digitalWrite(GREEN_LED_PIN, HIGH);
  strip.setPixelColor(0, strip.Color(0, 255, 0));
  strip.setPixelColor(2, strip.Color(0, 255, 0));
  strip.setPixelColor(4, strip.Color(0, 255, 0));
  strip.setPixelColor(6, strip.Color(0, 255, 0));
  strip.show();
  delay(500); // 0.5 second delay
  strip.clear();
  strip.show();
}

// Clear OK status
void clearOK() {
  digitalWrite(GREEN_LED_PIN, LOW);
  strip.clear();
  strip.show();
}