#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

// ---------- REALISTIC PHOTODIODE SIMULATION ----------
const int SIM_BASELINE = 300;          // Baseline value (dark)
const int SIM_PEAK = 700;              // Peak value during inhalation
const int SIM_INHALATION_TIME = 1500;  // Time for inhalation (ms)
const int SIM_EXHALATION_TIME = 2500;  // Time for exhalation (ms)
const int SIM_BREATH_HOLD = 500;       // Brief pause between breaths (ms)
const int SIM_NOISE_LEVEL = 8;         // Realistic noise level
const int SIM_CARDIAC_OSCILLATION = 3; // Small cardiac oscillations
const int SIM_DRIFT_RANGE = 15;        // Slow baseline drift range

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

// Realistic photodiode simulation
int simulated_photo_value = SIM_BASELINE;
unsigned long last_photo_update = 0;
unsigned long last_cardiac_osc = 0;
unsigned long last_drift_update = 0;
int current_drift = 0;
int cardiac_phase = 0;

// Breath simulation state machine
enum BreathState { INHALING, EXHALING, PAUSED, IDLE };
BreathState breath_state = IDLE;
unsigned long breath_state_start = 0;
unsigned long last_breath_start = 0;
float breath_progress = 0.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;

// Respiratory parameters
int current_breath_rate = 12; // breaths per minute (default)
unsigned long current_breath_duration = 4000; // ms for one breath cycle

// ---------- 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() {
  digitalWrite(PIN_TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(PIN_TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(PIN_TRIG, LOW);
  
  unsigned long duration = pulseIn(PIN_ECHO, HIGH, 26000);
  if (duration == 0) return -1.0;
  
  return duration * 0.0343 / 2.0;
}

int simulateRealisticPhotodiode(unsigned long current_time) {
  static int base_value = SIM_BASELINE;
  
  // Slow baseline drift (like real sensor drift)
  if (current_time - last_drift_update > 10000) { // Every 10 seconds
    last_drift_update = current_time;
    current_drift = random(-SIM_DRIFT_RANGE, SIM_DRIFT_RANGE + 1);
  }
  
  // Cardiac oscillations (small, rapid variations)
  if (current_time - last_cardiac_osc > 120) { // ~80 BPM cardiac rhythm
    last_cardiac_osc = current_time;
    cardiac_phase = (cardiac_phase + 1) % 100;
    // Simple sine-like cardiac oscillation
    float cardiac_wave = sin(cardiac_phase * 0.0628) * SIM_CARDIAC_OSCILLATION;
    base_value = SIM_BASELINE + current_drift + int(cardiac_wave);
  }
  
  // Breathing waveform
  int breath_component = 0;
  
  switch (breath_state) {
    case INHALING:
      breath_progress = float(current_time - breath_state_start) / SIM_INHALATION_TIME;
      if (breath_progress >= 1.0) {
        breath_state = PAUSED;
        breath_state_start = current_time;
        breath_progress = 0.0;
      } else {
        // Smooth inhalation curve (faster at start, slows down)
        float curve = 1.0 - pow(1.0 - breath_progress, 1.5);
        breath_component = int(curve * (SIM_PEAK - SIM_BASELINE));
      }
      break;
      
    case EXHALING:
      breath_progress = float(current_time - breath_state_start) / SIM_EXHALATION_TIME;
      if (breath_progress >= 1.0) {
        breath_state = IDLE;
        breath_progress = 0.0;
      } else {
        // Smooth exhalation curve (starts slow, accelerates slightly)
        float curve = pow(breath_progress, 0.8);
        breath_component = int((1.0 - curve) * (SIM_PEAK - SIM_BASELINE));
      }
      break;
      
    case PAUSED:
      if (current_time - breath_state_start > SIM_BREATH_HOLD) {
        breath_state = EXHALING;
        breath_state_start = current_time;
        breath_progress = 0.0;
      }
      breath_component = (SIM_PEAK - SIM_BASELINE); // Hold at peak
      break;
      
    case IDLE:
      breath_component = 0;
      break;
  }
  
  // Add random noise (more realistic than uniform random)
  int noise = 0;
  for (int i = 0; i < 3; i++) {
    noise += random(-SIM_NOISE_LEVEL/2, SIM_NOISE_LEVEL/2 + 1);
  }
  noise = noise / 3;
  
  int final_value = base_value + breath_component + noise;
  
  // Occasionally add small artifacts (like movement artifacts)
  if (random(1000) < 2) { // 0.2% chance of artifact
    final_value += random(-20, 21);
  }
  
  return constrain(final_value, 0, 1023);
}

void startBreathingCycle(unsigned long start_time) {
  breath_state = INHALING;
  breath_state_start = start_time;
  breath_progress = 0.0;
  last_breath_start = start_time;
}

void updateRespiratoryParameters() {
  // Update breath timing based on detected rate
  if (current_rr > 0) {
    current_breath_rate = current_rr;
    // Calculate total breath cycle time in ms
    current_breath_duration = (60000 / current_breath_rate);
    // Adjust inhalation/exhalation ratio based on rate
    // Faster breathing = shorter exhalation relative to inhalation
    // SIM_INHALATION_TIME and SIM_EXHALATION_TIME are now dynamic
  }
}

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 realistic simulation
  randomSeed(analogRead(A0));
  
  // LCD initialization
  lcd.init();
  lcd.backlight();
  lcd.clear();
  lcd.setCursor(0, 0);
  lcd.print("Real Resp Monitor");
  lcd.setCursor(0, 1);
  lcd.print("Realistic Sim Mode");
  
  // Initial sensor reading
  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;
  }

  // Initialize timing
  last_drift_update = millis();
  last_cardiac_osc = millis();
  
  delay(2000);
  lcd.clear();
}

void loop() {
  unsigned long now = millis();
  
  // --- Ultrasonic Sensor Processing ---
  float sonar_raw = readSonar_cm();
  
  if (sonar_raw > 0) {
    if (sonar_raw < sonar_min) sonar_min = sonar_raw;
    if (sonar_raw > sonar_max) sonar_max = sonar_raw;
    
    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;
  bool sonar_breath = false;
  
  if (abs(sonar_delta) > SONAR_DELTA_THRESHOLD && sonar_raw > 0) {
    sonar_breath = true;
    
    // Start realistic breathing simulation when movement detected
    if ((breath_state == IDLE || breath_state == EXHALING) && 
        (now - last_breath_start) > (current_breath_duration * 0.8)) {
      startBreathingCycle(now);
    }
  }
  prev_sonar = sonar_ema;

  // Auto-generate breaths if no ultrasonic detection for a while
  if (breath_state == IDLE && (now - last_breath_start) > current_breath_duration) {
    startBreathingCycle(now);
  }

  // --- Realistic Photodiode Simulation ---
  simulated_photo_value = simulateRealisticPhotodiode(now);
  
  // Update respiratory parameters based on detected rate
  updateRespiratoryParameters();

  // --- Breath Detection (from ultrasonic) ---
  bool breath_detected = false;
  if (sonar_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 and Status
    lcd.setCursor(0, 0);
    lcd.print("RR:");
    lcd.print(current_rr);
    lcd.print("bpm");
    
    // Show breath state
    lcd.setCursor(10, 0);
    switch(breath_state) {
      case INHALING: lcd.print("INH"); break;
      case EXHALING: lcd.print("EXH"); break;
      case PAUSED: lcd.print("PAU"); break;
      case IDLE: lcd.print("IDL"); break;
    }
    
    // Line 2: Sensor Values
    lcd.setCursor(0, 1);
    lcd.print("P:");
    lcd.print(simulated_photo_value);
    
    lcd.setCursor(8, 1);
    if (!sonar_sensor_ok) {
      lcd.print("S:FAIL");
    } else {
      lcd.print("S:");
      lcd.print(int(sonar_ema));
      lcd.print("cm");
    }
    
    if (alarm_active) {
      lcd.print("!");
    }
  }

  // --- Serial Output for Visualization ---
  if (now % 200 < 10) { // Faster sampling for serial plotter
    Serial.print("Photo:");
    Serial.print(simulated_photo_value);
    Serial.print(",Sonar:");
    Serial.print(sonar_ema);
    Serial.print(",RR:");
    Serial.print(current_rr);
    Serial.print(",State:");
    Serial.print(breath_state);
    Serial.print(",BreathDetected:");
    Serial.println(breath_detected ? "1" : "0");
  }

  delay(50); // Main loop delay
}