The tested prototype.

/**
 * ranging.h, Shared ultrasonic time-of-flight ranging
 *
 * getDistance() is identical across Node A (glasses), Node B (shoes),
 * and Node C (gloves), all three use a single HC-SR04-class sensor
 * read per call. Extracted here so the three nodes stay in sync.
 *
 * Round-trip formula: distance (cm) = pulseWidth (µs) / 58
 * (Speed of sound: ~343 m/s = 0.0343 cm/µs = 1 cm per 29 µs round-trip)
 */
#pragma once

// Sensor timeout (prevent infinite waits)
#define SENSOR_TIMEOUT 30000  // 30 ms in microseconds

/**
 * getDistance(), Measure distance using ultrasonic time-of-flight
 *
 * Returns distance in cm, or -1 on timeout / out-of-range reading
 * (valid range: 2 cm to 400 cm, per HC-SR04 datasheet).
 */
inline float getDistance(int trigPin, int echoPin) {
  // Send 10 µs trigger pulse
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  // Measure echo pulse width
  long pulseDuration = pulseIn(echoPin, HIGH, SENSOR_TIMEOUT);

  // Invalid measurement (sensor not responding or timeout)
  if (pulseDuration == 0) {
    return -1;  // Return negative to indicate error
  }

  // Convert pulse duration to distance
  float distance = pulseDuration / 58.0;

  // Clamp to reasonable range (2 cm to 400 cm)
  if (distance < 2 || distance > 400) {
    return -1;
  }

  return distance;
}

/**
 * Third Eye Kit, Node A: Upper Ranging (Glasses/Headwear)
 *
 * Detects obstacles at/above chest height.
 * Feedback: Local audio cue through headphone jack.
 * Status: [IMPLEMENTED]
 *
 * Platform: Arduino Mini Pro (3.3V or 5V variant)
 * Sensor: HC-SR04 Ultrasonic
 * Power: Lithium cell (1.5V × 1) or AA battery
 */

#include "../common/ranging.h"

// Pin Configuration
#define TRIG_PIN  9   // Trigger pin for HC-SR04
#define ECHO_PIN  10  // Echo pin for HC-SR04
#define AUDIO_PIN 11  // Audio feedback (PWM-capable)

// Configuration (tunable per user and environment)
#define ALERT_CM  70  // Alert distance in centimeters (~70 cm for head-height obstacle)

// Audio feedback configuration
#define AUDIO_FREQ    440  // Hz (A4 note)
#define AUDIO_DURATION 200  // ms

// Forward declarations
void playAudio(int pin, int freq, int duration);

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

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(AUDIO_PIN, OUTPUT);

  // Optional: indicator LED to show power-on
  digitalWrite(AUDIO_PIN, LOW);

  Serial.println("Node A: Upper Ranging System");
  Serial.print("Alert distance: ");
  Serial.print(ALERT_CM);
  Serial.println(" cm");
  Serial.println("Waiting for obstacles...");
}

void loop() {
  // Get distance from ultrasonic sensor
  float distance = getDistance(TRIG_PIN, ECHO_PIN);

  // Check if obstacle is within alert range
  if (distance > 0 && distance < ALERT_CM) {
    // Obstacle detected, play audio alert
    playAudio(AUDIO_PIN, AUDIO_FREQ, AUDIO_DURATION);
    Serial.print("ALERT: Obstacle at ");
    Serial.print(distance);
    Serial.println(" cm");
  } else if (distance > 0) {
    // Optional: log distance periodically for diagnostics
    // Serial.print("Distance: "); Serial.println(distance);
  }

  delay(100);  // Sampling interval (10 Hz)
}

/**
 * playAudio(), Generate audio tone feedback via PWM
 *
 * Note: Simple square-wave tone. For better audio quality,
 * use hardware-based tone generation or pre-recorded samples.
 */
void playAudio(int pin, int freq, int duration) {
  int period = 1000000 / freq;  // Period in microseconds
  int halfPeriod = period / 2;
  long endTime = millis() + duration;

  while (millis() < endTime) {
    digitalWrite(pin, HIGH);
    delayMicroseconds(halfPeriod);
    digitalWrite(pin, LOW);
    delayMicroseconds(halfPeriod);
  }

  digitalWrite(pin, LOW);  // Ensure pin is low after audio stops
}

/**
 * Third Eye Kit, Node B: Lower Ranging (Shoes)
 *
 * Dual ultrasonic sensors for left and right foot.
 * Detects ground-plane hazards (curbs, holes, steps).
 * Feedback: Haptic vibration motors + passive LED visibility.
 * Status: [IMPLEMENTED]
 *
 * Platform: Arduino Mini Pro (5V)
 * Sensors: 2 × HC-SR04 Ultrasonic
 * Actuators: 2 × Vibration motors, LDR + LED
 */

#include "../common/ranging.h"

// Left Shoe Sensor
#define LEFT_TRIG  9
#define LEFT_ECHO  10

// Right Shoe Sensor
#define RIGHT_TRIG 3
#define RIGHT_ECHO 4

// Haptic & LED
#define LEFT_HAPTIC  5   // PWM
#define RIGHT_HAPTIC 6   // PWM
#define LDR_PIN      A0  // Analog input
#define LED_PIN      7   // Digital output

// Configuration
#define ALERT_CM_LEFT  60   // cm, tune per user stride
#define ALERT_CM_RIGHT 60   // cm
#define LED_THRESHOLD  300  // Ambient light threshold (0-1023 ADC)

void activateHaptic(int pin, int duration);

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

  // Sensor pins
  pinMode(LEFT_TRIG, OUTPUT);
  pinMode(LEFT_ECHO, INPUT);
  pinMode(RIGHT_TRIG, OUTPUT);
  pinMode(RIGHT_ECHO, INPUT);

  // Haptic & LED
  pinMode(LEFT_HAPTIC, OUTPUT);
  pinMode(RIGHT_HAPTIC, OUTPUT);
  pinMode(LED_PIN, OUTPUT);

  Serial.println("Node B: Lower Ranging System (Dual Shoe)");
  Serial.print("Left alert: ");
  Serial.print(ALERT_CM_LEFT);
  Serial.println(" cm");
  Serial.print("Right alert: ");
  Serial.print(ALERT_CM_RIGHT);
  Serial.println(" cm");
}

void loop() {
  float leftDist = getDistance(LEFT_TRIG, LEFT_ECHO);
  float rightDist = getDistance(RIGHT_TRIG, RIGHT_ECHO);

  // Left shoe hazard detection
  if (leftDist > 0 && leftDist < ALERT_CM_LEFT) {
    activateHaptic(LEFT_HAPTIC, 100);
    Serial.print("LEFT: ");
    Serial.print(leftDist);
    Serial.println(" cm");
  }

  // Right shoe hazard detection
  if (rightDist > 0 && rightDist < ALERT_CM_RIGHT) {
    activateHaptic(RIGHT_HAPTIC, 100);
    Serial.print("RIGHT: ");
    Serial.print(rightDist);
    Serial.println(" cm");
  }

  // Ambient light detection for passive visibility (LDR + LED)
  int ambientLight = analogRead(LDR_PIN);
  if (ambientLight < LED_THRESHOLD) {
    digitalWrite(LED_PIN, HIGH);  // Turn on LED in darkness
  } else {
    digitalWrite(LED_PIN, LOW);
  }

  delay(100);  // Sampling interval (10 Hz)
}

void activateHaptic(int pin, int duration) {
  digitalWrite(pin, HIGH);
  delay(duration);
  digitalWrite(pin, LOW);
}

/**
 * Third Eye Kit, Node C: Manual/Tactile Scanning (Gloves)
 *
 * Near-field obstacle detection for hand-guided exploration.
 * Feedback: Low-intensity haptic.
 * Status: [IN DEV], Glove enclosure not yet fabricated.
 *
 * Platform: Arduino Mini Pro (3.3V)
 * Sensor: 1 × HC-SR04 Ultrasonic
 * Actuator: 1 × Vibration motor
 */

#include "../common/ranging.h"

#define TRIG_PIN      9
#define ECHO_PIN      10
#define HAPTIC_PIN    5

// Near-field range (arm's reach)
#define ALERT_CM      40   // cm, detect objects within arm's reach

void setup() {
  Serial.begin(9600);
  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);
  pinMode(HAPTIC_PIN, OUTPUT);

  Serial.println("Node C: Manual/Tactile Scanning [IN DEV]");
  Serial.println("Glove enclosure design pending.");
}

void loop() {
  float distance = getDistance(TRIG_PIN, ECHO_PIN);

  if (distance > 0 && distance < ALERT_CM) {
    // Low-intensity haptic pulse
    digitalWrite(HAPTIC_PIN, HIGH);
    delayMicroseconds(50);  // Very short pulse for subtle feedback
    digitalWrite(HAPTIC_PIN, LOW);

    Serial.print("Object at ");
    Serial.print(distance);
    Serial.println(" cm (hand range)");
  }

  delay(50);  // Higher sample rate for hand-guided exploration
}

/**
 * Third Eye Kit, Node D: Location Telemetry (Belt/Pack)
 *
 * GPS positioning + GSM location reporting to authorized family members.
 * Feedback: Outbound SMS with coordinates.
 * Status: [IMPLEMENTED], Call-triggered; see consent notes below.
 *
 * Platform: Arduino Mini Pro (5V)
 * Modules: NEO-6M GPS + SIM800L GSM
 * Power: Lithium cell
 *
 * PRIVACY & CONSENT:
 * This node responds to inbound requests from authorized numbers.
 * See docs/CONSENT_ARCHITECTURE.md for design rationale and limitations.
 * Current v2 baseline: answers inbound requests.
 * Roadmap: wearer-controlled toggle to milestone all location sharing.
 */

#include <SoftwareSerial.h>

// GPS Module (NEO-6M)
// RX = pin 0 (hardware Serial), TX = pin 1
#define GPS_BAUD 9600

// GSM Module (SIM800L)
#define GSM_RX_PIN 2
#define GSM_TX_PIN 3
#define GSM_BAUD 9600
#define GSM_KEY_PIN 8

SoftwareSerial gsmSerial(GSM_RX_PIN, GSM_TX_PIN);  // RX, TX

// Authorized phone numbers (one-time configuration)
const char* AUTHORIZED[] = {
  "+93799123456",  // Family member 1 (update with real number)
  "+93799654321"   // Family member 2 (update with real number)
};
const int AUTH_COUNT = 2;

// Global state
float lastLat = 0, lastLon = 0;
bool gpsFixed = false;

void setup() {
  Serial.begin(9600);     // Debug serial (USB)
  gsmSerial.begin(GSM_BAUD);  // GSM module

  pinMode(GSM_KEY_PIN, OUTPUT);
  digitalWrite(GSM_KEY_PIN, LOW);

  Serial.println("Node D: Location Telemetry System");
  Serial.println("Initializing GPS and GSM modules...");

  // Power on GSM module
  initializeGSM();

  // Note: Real implementation would initialize GPS and wait for first fix
  // This is a skeleton; full implementation requires GPS NMEA parsing
}

void loop() {
  // Poll for incoming SMS from authorized numbers
  checkIncomingSMS();

  // Update GPS periodically (every 30 seconds in production)
  static unsigned long lastGPSUpdate = 0;
  if (millis() - lastGPSUpdate > 30000) {
    updateGPSPosition();
    lastGPSUpdate = millis();
  }

  delay(100);
}

/**
 * initializeGSM(), Power on and configure GSM module
 */
void initializeGSM() {
  // Toggle power pin (at least 1 second)
  digitalWrite(GSM_KEY_PIN, HIGH);
  delay(1000);
  digitalWrite(GSM_KEY_PIN, LOW);

  delay(2000);  // Wait for module to boot

  // Check if module responds
  sendAT("AT");
  // Expected: "OK"

  // Disable command echo
  sendAT("ATE0");

  // Set SMS text mode
  sendAT("AT+CMGF=1");

  Serial.println("GSM module initialized.");
}

/**
 * checkIncomingSMS(), Poll for incoming SMS
 */
void checkIncomingSMS() {
  // In production: parse unsolicited +CMT notifications
  // Simplified skeleton: check periodically for new messages
  sendAT("AT+CMGL=\"REC UNREAD\"");

  // If message received from authorized number, reply with location
  // See CONSENT_ARCHITECTURE.md for privacy design rationale
}

/**
 * updateGPSPosition(), Read GPS coordinates
 */
void updateGPSPosition() {
  // In production: parse NMEA frames from NEO-6M
  // Example: $GPRMC,hhmmss.ss,status,lat,N/S,lon,E/W,...
  // Simplified skeleton; real implementation uses full NMEA parser

  // For now, log that GPS update was attempted
  Serial.println("GPS: Checking for fix...");

  // When fix is obtained, send to authorized requester
  // sendLocationSMS("+93799123456", lastLat, lastLon);
}

/**
 * sendLocationSMS(), Send GPS coordinates via SMS
 */
void sendLocationSMS(const char* phoneNumber, float lat, float lon) {
  // Compose SMS with coordinates
  String message = "Location: ";
  message += lat;
  message += ",";
  message += lon;

  // Send SMS command
  String command = "AT+CMGS=\"";
  command += phoneNumber;
  command += "\"";

  sendAT(command);
  // Send message content followed by Ctrl+Z (0x1A)
  gsmSerial.print(message);
  gsmSerial.write(0x1A);

  Serial.print("Location sent to ");
  Serial.println(phoneNumber);
}

/**
 * sendAT(), Send AT command and read response
 */
void sendAT(const char* command) {
  gsmSerial.println(command);

  // Wait for response (simplified; real implementation has timeout)
  delay(500);

  // Read response
  while (gsmSerial.available()) {
    Serial.write(gsmSerial.read());
  }
}

// ============================================================================
// CONSENT DESIGN RATIONALE
// ============================================================================
//
// The v2 prototype answers inbound requests from authorized numbers.
// This is convenient but NOT a full consent milestone.
//
// Limitations (v2):
// - Wearer cannot disable sharing while carrying the device
// - Abusive caller can query location repeatedly
// - Family member could disable without wearer's knowledge (if device is accessible)
//
// Roadmap [PLANNED]:
// - Physical toggle on device body (wearer controls)
// - Firmware allowlist (only pre-authorized numbers can request)
// - Rate limiting (prevent continuous location tracking)
// - No remote provisioning (no way to add numbers without physical access)
//
// See docs/CONSENT_ARCHITECTURE.md for full design.