// Respiratory monitoring with HC-SR04 + Simulated Photodiode + I2C LCD
// Arduino UNO compatible
// Photodiode simulation based on ultrasonic movement

#include <LiquidCrystal_I2C.h>

// ---------- PIN CONFIG ----------
const int PIN_TRIG  = 2;      // HC-SR04 trigger
const int PIN_ECHO  = 3;      // HC-SR04 echo
const int PIN_BUZZ  = 13;     // Buzzer or LED indicator

// ---------- LCD (I2C) ----------
LiquidCrystal_I2C lcd(0x27, 16, 2); // Use 0x3F if 0x27 doesn't work

// ---------- TUNEABLE PARAMETERS ----------
const float SONAR_SMOOTHING = 0.1;     // EMA alpha for ultrasonic
const unsigned long REFRACTORY_MS = 800;     // Min ms between breaths
const unsigned long BREATH_WINDOW_SECONDS = 45; // Sliding window for BPM calculation
const float SONAR_DELTA_THRESHOLD = 1.2; // Min cm change for breath detection
const int ALARM_RR_LOW = 8;            // Low respiratory rate alarm
const int ALARM_RR_HIGH = 30;          // High respiratory rate alarm

// ---------- PHOTODIODE SIMULATION PARAMETERS ----------
const int SIM_BASELINE = 500;          // Baseline value for simulated photodiode
const int SIM_AMPLITUDE = 80;          // Amplitude of breathing signal
const int SIM_BREATH_DURATION = 3000;  // Duration of simulated breath in ms
const int SIM_NOISE_LEVEL = 5;         // Random noise level

// ---------- GLOBAL STATE ----------
// Ultrasonic sensor
float sonar_ema = 0.0;
float sonar_min = 999, sonar_max = -999;

// Simulated photodiode
int simulated_photo_value = SIM_BASELINE;
bool is_breathing_cycle = false;
unsigned long breath_start_time = 0;
unsigned long last_photo_update = 0;

// Breath tracking
unsigned long last_breath_ms = 0;

// Breath history for rate calculation
const int MAX_BREATHS = 100;
unsigned long breath_times[MAX_BREATHS];
int breath_head = 0;
int breath_count = 0;

// Display and timing
unsigned long last_display_ms = 0;
const unsigned long DISPLAY_INTERVAL = 800;

// System state
bool alarm_active = false;
int current_rr = 0;
bool sonar_sensor_ok = true;

// ---------- HELPER FUNCTIONS ----------
void add_breath_timestamp(unsigned long t) {
  breath_times[breath_head] = t;
  breath_head = (breath_head + 1) % MAX_BREATHS;
  if (breath_count < MAX_BREATHS) breath_count++;
}

int count_breaths_in_window(unsigned long now_ms, unsigned long window_seconds) {
  if (breath_count == 0) return 0;
  
  unsigned long cutoff = now_ms - window_seconds * 1000UL;
  int cnt = 0;
  
  for (int i = 0; i < breath_count; ++i) {
    int idx = (breath_head - 1 - i + MAX_BREATHS) % MAX_BREATHS;
    if (breath_times[idx] >= cutoff) {
      cnt++;
    } else {
      break;
    }
  }
  return cnt;
}

float readSonar_cm() {
  // Trigger pulse with proper timing
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  
  // Read echo with timeout (26ms = ~4.5m max distance)
  unsigned long duration = pulseIn(PIN_ECHO, HIGH, 26000);
  if (duration == 0) return -1.0; // Timeout or no echo
  
  // Convert to cm (sound travels ~343 m/s = 0.0343 cm/µs)
  return duration * 0.0343 / 2.0;
}

int simulatePhotodiodeValue(unsigned long current_time) {
  if (!is_breathing_cycle) {
    // Return baseline with small random variations
    return SIM_BASELINE + random(-SIM_NOISE_LEVEL, SIM_NOISE_LEVEL + 1);
  }
  
  // Calculate progress through breathing cycle (0.0 to 1.0)
  float progress = (float)(current_time - breath_start_time) / SIM_BREATH_DURATION;
  
  if (progress >= 1.0) {
    // Breathing cycle complete
    is_breathing_cycle = false;
    return SIM_BASELINE + random(-SIM_NOISE_LEVEL, SIM_NOISE_LEVEL + 1);
  }
  
  // Create a smooth breathing waveform (sine wave)
  float waveform = sin(progress * 3.14159); // 0 to PI radians
  int simulated_value = SIM_BASELINE + (waveform * SIM_AMPLITUDE);
  
  // Add some random noise
  simulated_value += random(-SIM_NOISE_LEVEL, SIM_NOISE_LEVEL + 1);
  
  // Constrain to reasonable ADC range
  return constrain(simulated_value, 0, 1023);
}

void startBreathingSimulation(unsigned long start_time) {
  is_breathing_cycle = true;
  breath_start_time = start_time;
}

void update_alarm_state() {
  bool rr_out_of_range = (current_rr > 0 && (current_rr < ALARM_RR_LOW || current_rr > ALARM_RR_HIGH));
  alarm_active = !sonar_sensor_ok || rr_out_of_range;
  digitalWrite(PIN_BUZZ, alarm_active ? HIGH : LOW);
}

void setup() {
  Serial.begin(115200);
  
  // Pin setup
  pinMode(PIN_TRIG, OUTPUT);
  pinMode(PIN_ECHO, INPUT);
  pinMode(PIN_BUZZ, OUTPUT);
  digitalWrite(PIN_BUZZ, LOW);
  digitalWrite(PIN_TRIG, LOW);

  // Initialize random seed for photodiode simulation
  randomSeed(analogRead(A0));
  
  // LCD initialization
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Respiratory Monitor");
  lcd.setCursor(0, 1);
  lcd.print("SIM Photo Mode");
  
  // Initial sensor reading for EMA
  delay(100);
  float initial_sonar = readSonar_cm();
  if (initial_sonar > 0) {
    sonar_ema = initial_sonar;
    sonar_min = initial_sonar;
    sonar_max = initial_sonar;
  } else {
    sonar_ema = 50.0; // Default fallback
  }

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

void loop() {
  unsigned long now = millis();
  
  // --- Ultrasonic Sensor Processing ---
  float sonar_raw = readSonar_cm();
  
  if (sonar_raw > 0) { // Valid reading
    // Update min/max
    if (sonar_raw < sonar_min) sonar_min = sonar_raw;
    if (sonar_raw > sonar_max) sonar_max = sonar_raw;
    
    // EMA smoothing
    sonar_ema = (1.0 - SONAR_SMOOTHING) * sonar_ema + SONAR_SMOOTHING * sonar_raw;
    sonar_sensor_ok = true;
  } else {
    sonar_sensor_ok = false;
  }

  // Detect breath from sonar movement
  static float prev_sonar = sonar_ema;
  float sonar_delta = prev_sonar - sonar_ema; // Positive when moving closer
  bool sonar_breath = false;
  
  if (abs(sonar_delta) > SONAR_DELTA_THRESHOLD && sonar_raw > 0) {
    sonar_breath = true;
    
    // Start photodiode simulation when movement detected
    if (!is_breathing_cycle && (now - last_breath_ms) > REFRACTORY_MS) {
      startBreathingSimulation(now);
    }
  }
  prev_sonar = sonar_ema;

  // --- Simulated Photodiode Processing ---
  simulated_photo_value = simulatePhotodiodeValue(now);
  
  // Detect breath from simulated photodiode (for demonstration)
  static int prev_sim_photo = simulated_photo_value;
  int sim_delta = simulated_photo_value - prev_sim_photo;
  bool sim_breath = false;
  
  // Detect rising edge in simulated signal
  if (sim_delta > 10 && is_breathing_cycle) {
    sim_breath = true;
  }
  prev_sim_photo = simulated_photo_value;

  // --- Breath Detection (Primary: Ultrasonic, Secondary: Simulated Photo) ---
  bool breath_detected = false;
  if ((sonar_breath || sim_breath) && (now - last_breath_ms) > REFRACTORY_MS) {
    breath_detected = true;
    last_breath_ms = now;
    add_breath_timestamp(now);
  }

  // --- Respiratory Rate Calculation ---
  int breaths_in_window = count_breaths_in_window(now, BREATH_WINDOW_SECONDS);
  current_rr = round(breaths_in_window * (60.0 / BREATH_WINDOW_SECONDS));

  // --- Alarm Management ---
  update_alarm_state();

  // --- Periodic Display Update ---
  if (now - last_display_ms >= DISPLAY_INTERVAL) {
    last_display_ms = now;
    
    lcd.clear();
    
    // Line 1: Respiratory Rate
    lcd.setCursor(0, 0);
    lcd.print("RR:");
    lcd.print(current_rr);
    lcd.print(" bpm");
    
    if (alarm_active) {
      lcd.print(" ALARM!");
    }
    
    // Line 2: Sensor Status and Values
    lcd.setCursor(0, 1);
    if (!sonar_sensor_ok) {
      lcd.print("Sonar:FAIL");
    } else {
      lcd.print("S:");
      lcd.print(int(sonar_ema));
      lcd.print("cm");
      
      // Show simulated photodiode value
      lcd.print(" P:");
      lcd.print(simulated_photo_value);
    }
  }

  // --- Serial Output for Debugging ---
  if (now % 2000 < 50) { // Every ~2 seconds
    Serial.print("Sonar: ");
    Serial.print(sonar_ema);
    Serial.print("cm Delta: ");
    Serial.print(sonar_delta);
    Serial.print(" | Sim Photo: ");
    Serial.print(simulated_photo_value);
    Serial.print(" | RR: ");
    Serial.print(current_rr);
    Serial.print(" bpm | Breathing: ");
    Serial.print(is_breathing_cycle ? "YES" : "NO");
    Serial.print(" | Alarm: ");
    Serial.println(alarm_active ? "ON" : "OFF");
  }

  delay(50); // Main loop delay
}