Med Matrix Sentinel Wearable

#include <Arduino.h>
#include <Wire.h>
#include <BLEDevice.h>
#include <BLEServer.h>
#include <BLEUtils.h>
#include <BLE2902.h>
#include <Adafruit_TCA9548A.h>
#include <Adafruit_DRV2605.h>
#include <Adafruit_ADXL345_U.h>
#include <Adafruit_TMP117.h>
#include <Adafruit_BME280.h>
#include <SparkFun_BMV080_Arduino_Library.h>
#include <MAX30105.h>
#include "heartRate.h"


// Forward declarations
void selectMuxChannel(uint8_t channel);
int pm25ToAqi(float p);
void buzz(uint8_t effect);
void publish();
void setRisk(const String &nextRisk);
void emergency(const char *reason);
void sampleVitalsAndMotion();
void sampleAir();
void evaluate();
void setupBle();

constexpr int I2C_SDA = 6;
constexpr int I2C_SCL = 7;
constexpr int SOS_BUTTON_PIN = 4;
constexpr uint8_t PPG_CHANNEL = 0;
constexpr uint8_t PM_CHANNEL = 1;
constexpr uint8_t BMV080_ADDR = 0x57;
constexpr uint8_t BME280_ADDR = 0x77;
constexpr uint32_t SAMPLE_MS = 25;
constexpr uint32_t AIR_MS = 1000;
constexpr uint32_t SOS_HOLD_MS = 2000;
constexpr float FALL_G = 2.35f;
constexpr float HIGH_SKIN_C = 38.0f;
constexpr float HIGH_AMBIENT_C = 40.0f;
constexpr uint32_t FINGER_IR_MIN = 50000;
const char BLE_SERVICE_UUID[] = "6b5b1c4d-6b86-4d4b-8b19-5f1c3a020001";
const char BLE_STATUS_UUID[] = "6b5b1c4d-6b86-4d4b-8b19-5f1c3a020002";

Adafruit_TCA9548A i2cMux;
Adafruit_DRV2605 haptic;
Adafruit_ADXL345_Unified accel(12345);
Adafruit_TMP117 tmp117;
Adafruit_BME280 bme;
SparkFunBMV080 bmv080;
MAX30105 ppg;
BLECharacteristic *statusChar = nullptr;
bool muxOK = false;
bool hapticOK = false;
bool accelOK = false;
bool tmpOK = false;
bool bmeOK = false;
bool pmOK = false;
bool ppgOK = false;
bool sos = false;
bool alertSent = false;
uint32_t buttonAt = 0;
uint32_t lastSample = 0;
uint32_t lastAir = 0;
uint32_t lastBeat = 0;
float hr = 0;
float spo2 = 0;
float skinC = 0;
float ambientC = 0;
float humidity = 0;
float pm25 = 0;
float motionG = 1;
int aqi = -1;
String risk = "STARTING";
String alertReason = "";

void selectMuxChannel(uint8_t channel) {
  if (!muxOK) return;
  i2cMux.closeAll();
  i2cMux.openChannel(channel);
}

int pm25ToAqi(float p) {
  if (p < 0) return -1;
  struct Band { float low; float high; int aqiLow; int aqiHigh; };
  const Band bands[] = {{0, 12, 0, 50}, {12.1f, 35.4f, 51, 100}, {35.5f, 55.4f, 101, 150}, {55.5f, 150.4f, 151, 200}, {150.5f, 250.4f, 201, 300}, {250.5f, 350.4f, 301, 400}, {350.5f, 500.4f, 401, 500}};
  p = floorf(p * 10.0f) / 10.0f;
  if (p > 500.4f) return 500;
  for (const auto &band : bands) {
    if (p >= band.low && p <= band.high) {
      return round(((float)(band.aqiHigh - band.aqiLow) / (band.high - band.low)) * (p - band.low) + band.aqiLow);
    }
  }
  return -1;
}

void buzz(uint8_t effect) {
  if (!hapticOK) return;
  haptic.setWaveform(0, effect);
  haptic.setWaveform(1, 0);
  haptic.go();
}

void publish() {
  if (!statusChar) return;
  String message = "state=" + risk + ",reason=" + alertReason + ",hr=" + String(hr, 0) + ",spo2=" + String(spo2, 0) + ",skinC=" + String(skinC, 1) + ",ambientC=" + String(ambientC, 1) + ",humidity=" + String(humidity, 0) + ",pm25=" + String(pm25, 1) + ",aqi=" + String(aqi);
  statusChar->setValue(message.c_str());
  statusChar->notify();
}

void setRisk(const String &nextRisk) {
  if (nextRisk == risk) return;
  risk = nextRisk;
  if (risk == "SOS") buzz(82);
  else if (risk == "ALERT") buzz(47);
  publish();
}

void emergency(const char *reason) {
  if (alertSent) return;
  alertSent = true;
  alertReason = reason;
  setRisk("SOS");
}

void sampleVitalsAndMotion() {
  if (ppgOK) {
    selectMuxChannel(PPG_CHANNEL);
    ppg.check();
    while (ppg.available()) {
      uint32_t ir = ppg.getIR();
      if (ir > FINGER_IR_MIN && checkForBeat(ir)) {
        uint32_t now = millis();
        if (lastBeat != 0) {
          float bpm = 60.0f / ((now - lastBeat) / 1000.0f);
          if (bpm >= 35.0f && bpm <= 220.0f) hr = (hr == 0) ? bpm : (0.8f * hr + 0.2f * bpm);
        }
        lastBeat = now;
      }
      if (ir <= FINGER_IR_MIN) {
        hr = 0;
        spo2 = 0;
        lastBeat = 0;
      }
      ppg.nextSample();
    }
  }
  if (accelOK) {
    sensors_event_t event;
    accel.getEvent(&event);
    motionG = sqrtf(event.acceleration.x * event.acceleration.x + event.acceleration.y * event.acceleration.y + event.acceleration.z * event.acceleration.z) / SENSORS_GRAVITY_STANDARD;
    if (motionG >= FALL_G) emergency("possible fall");
  }
  if (tmpOK) {
    sensors_event_t event;
    tmp117.getEvent(&event);
    if (event.temperature > 15 && event.temperature < 45) skinC = event.temperature;
  }
}

void sampleAir() {
  if (millis() - lastAir < AIR_MS) return;
  lastAir = millis();
  if (bmeOK) {
    float nextAmbientC = bme.readTemperature();
    float nextHumidity = bme.readHumidity();
    if (!isnan(nextAmbientC) && nextAmbientC > -20 && nextAmbientC < 70) ambientC = nextAmbientC;
    if (!isnan(nextHumidity) && nextHumidity >= 0 && nextHumidity <= 100) humidity = nextHumidity;
  }
  if (pmOK) {
    selectMuxChannel(PM_CHANNEL);
    if (bmv080.readSensor()) {
      pm25 = bmv080.PM25();
      aqi = pm25ToAqi(pm25);
    }
  }
  publish();
}

void evaluate() {
  if (sos || alertSent) return;
  bool risky = hr >= 130 || (hr >= 35 && hr <= 40) || skinC >= HIGH_SKIN_C || ambientC >= HIGH_AMBIENT_C || aqi >= 151;
  alertReason = risky ? "health, heat, or air-quality warning" : "";
  setRisk(risky ? "ALERT" : "OK");
}

void setupBle() {
  BLEDevice::init("MedMatrix-Sentinel");
  BLEServer *server = BLEDevice::createServer();
  BLEService *service = server->createService(BLE_SERVICE_UUID);
  statusChar = service->createCharacteristic(BLE_STATUS_UUID, BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY);
  statusChar->addDescriptor(new BLE2902());
  service->start();
  BLEAdvertising *advertising = BLEDevice::getAdvertising();
  advertising->addServiceUUID(BLE_SERVICE_UUID);
  advertising->start();
}

void setup() {
  Serial.begin(115200);
  pinMode(SOS_BUTTON_PIN, INPUT_PULLUP);
  Wire.begin(I2C_SDA, I2C_SCL);
  Wire.setClock(400000);
  muxOK = i2cMux.begin(0x70, &Wire);
  if (muxOK) {
    selectMuxChannel(PPG_CHANNEL);
    ppgOK = ppg.begin(Wire, I2C_SPEED_FAST);
    if (ppgOK) ppg.setup(0x1F, 4, 2, 100, 411, 4096);
    selectMuxChannel(PM_CHANNEL);
    pmOK = bmv080.begin(BMV080_ADDR, Wire);
    if (pmOK) {
      bmv080.init();
      pmOK = bmv080.setMode(SF_BMV080_MODE_CONTINUOUS);
    }
    i2cMux.closeAll();
  }
  hapticOK = haptic.begin();
  if (hapticOK) haptic.selectLibrary(1);
  accelOK = accel.begin();
  if (accelOK) accel.setRange(ADXL345_RANGE_16_G);
  tmpOK = tmp117.begin();
  bmeOK = bme.begin(BME280_ADDR, &Wire);
  setupBle();
  setRisk("OK");
  buzz(1);
}

void loop() {
  uint32_t now = millis();
  if (digitalRead(SOS_BUTTON_PIN) == LOW) {
    if (!buttonAt) buttonAt = now;
    if (!sos && now - buttonAt >= SOS_HOLD_MS) {
      sos = true;
      emergency("SOS button held");
    }
  } else {
    buttonAt = 0;
    sos = false;
  }
  if (now - lastSample >= SAMPLE_MS) {
    lastSample = now;
    sampleVitalsAndMotion();
    sampleAir();
    evaluate();
  }
}