#include <Arduino.h>
#include <Wire.h>
#include <DHT.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <math.h>

#define DHT1_PIN 4
#define DHT2_PIN 5
#define DHT3_PIN 18
#define JOLT1_PIN 13
#define JOLT2_PIN 14
#define JOLT3_PIN 27
#define BTN_VACCINE 23
#define BTN_BLOOD 19
#define BTN_ORGAN 17
#define BATT_PIN 34
#define LED_R_PIN 25
#define LED_G_PIN 26
#define LED_B_PIN 32
#define BUZZER_PIN 33
#define DHT_TYPE DHT22
#define SDA_PIN 21
#define SCL_PIN 22

const float TEMP_DEV_THRESHOLD = 1.5;
const float HUM_DEV_THRESHOLD = 8.0;
const float JOLT_QUORUM_FRACTION = 0.5;
const float SHOCK_THRESHOLD = 20.0;

enum ShipState {
  SAFE,
  WARNING,
  CRITICAL,
  LOCKED
};

struct CargoProfile {
  const char* name;
  float tempMin;
  float tempMax;
  float humMin;
  float humMax;
  unsigned long lockMs;
};

CargoProfile profiles[3] = {
  { "Vaccine", 2.0, 8.0, 20.0, 60.0, 20UL * 1000UL },
  { "Blood",   1.0, 6.0, 30.0, 70.0, 12UL * 1000UL },
  { "Organ",   4.0, 10.0, 30.0, 70.0, 8UL * 1000UL }
};

DHT dht1(DHT1_PIN, DHT_TYPE);
DHT dht2(DHT2_PIN, DHT_TYPE);
DHT dht3(DHT3_PIN, DHT_TYPE);

Adafruit_MPU6050 mpu;

int cargoMode = 0;

ShipState currentState = SAFE;

unsigned long criticalStart = 0;
unsigned long lastBuzzer = 0;
unsigned long lastSensorRead = 0;

bool lockActive = false;

float temperatures[3] = {5.0, 5.0, 5.0};
float humidities[3] = {40.0, 40.0, 40.0};

float trustedTemp = 5.0;
float trustedHum = 40.0;

bool tempSensorHealthy[3];
bool humSensorHealthy[3];

bool shockDetected = false;

float median3(float a, float b, float c) {
  if (a > b) {
    float t = a;
    a = b;
    b = t;
  }

  if (b > c) {
    float t = b;
    b = c;
    c = t;
  }

  if (a > b) {
    float t = a;
    a = b;
    b = t;
  }

  return b;
}

float medianVote(
  float values[3],
  bool healthy[3],
  float deviationThreshold
) {
  float med = median3(values[0], values[1], values[2]);

  int healthyCount = 0;
  float sum = 0;

  for (int i = 0; i < 3; i++) {
    healthy[i] =
      fabs(values[i] - med) <= deviationThreshold;

    if (healthy[i]) {
      sum += values[i];
      healthyCount++;
    }
  }

  if (healthyCount == 0) {
    return med;
  }

  return sum / healthyCount;
}

bool readEnvironment() {
  float t1 = dht1.readTemperature();
  float t2 = dht2.readTemperature();
  float t3 = dht3.readTemperature();

  float h1 = dht1.readHumidity();
  float h2 = dht2.readHumidity();
  float h3 = dht3.readHumidity();

  if (!isnan(t1)) temperatures[0] = t1;
  if (!isnan(t2)) temperatures[1] = t2;
  if (!isnan(t3)) temperatures[2] = t3;

  if (!isnan(h1)) humidities[0] = h1;
  if (!isnan(h2)) humidities[1] = h2;
  if (!isnan(h3)) humidities[2] = h3;

  trustedTemp =
    medianVote(
      temperatures,
      tempSensorHealthy,
      TEMP_DEV_THRESHOLD
    );

  trustedHum =
    medianVote(
      humidities,
      humSensorHealthy,
      HUM_DEV_THRESHOLD
    );

  return true;
}

bool detectShock() {
  sensors_event_t acceleration;
  sensors_event_t gyro;
  sensors_event_t temperature;

  mpu.getEvent(
    &acceleration,
    &gyro,
    &temperature
  );

  float magnitude = sqrt(
    acceleration.acceleration.x *
    acceleration.acceleration.x +

    acceleration.acceleration.y *
    acceleration.acceleration.y +

    acceleration.acceleration.z *
    acceleration.acceleration.z
  );

  return magnitude >= SHOCK_THRESHOLD;
}

bool outsideRange(float value, float minValue, float maxValue) {
  return value < minValue || value > maxValue;
}

bool nearBoundary(
  float value,
  float minValue,
  float maxValue
) {
  float range = maxValue - minValue;
  float margin = range * 0.15;

  return
    fabs(value - minValue) <= margin ||
    fabs(value - maxValue) <= margin;
}

ShipState evaluateState() {
  CargoProfile &profile = profiles[cargoMode];

  if (
    outsideRange(
      trustedTemp,
      profile.tempMin,
      profile.tempMax
    )
  ) {
    return CRITICAL;
  }

  if (
    nearBoundary(
      trustedTemp,
      profile.tempMin,
      profile.tempMax
    )
  ) {
    return WARNING;
  }

  if (
    outsideRange(
      trustedHum,
      profile.humMin,
      profile.humMax
    )
  ) {
    return CRITICAL;
  }

  if (
    nearBoundary(
      trustedHum,
      profile.humMin,
      profile.humMax
    )
  ) {
    return WARNING;
  }

  if (detectShock()) {
    return CRITICAL;
  }

  int rawBattery = analogRead(BATT_PIN);

  float batteryPercent =
    ((float)rawBattery / 4095.0) * 100.0;

  if (batteryPercent <= 10.0) {
    return CRITICAL;
  }

  if (batteryPercent <= 25.0) {
    return WARNING;
  }

  return SAFE;
}

void setRGB(bool r, bool g, bool b) {
  digitalWrite(LED_R_PIN, r ? HIGH : LOW);
  digitalWrite(LED_G_PIN, g ? HIGH : LOW);
  digitalWrite(LED_B_PIN, b ? HIGH : LOW);
}

void applyStateOutput(ShipState state) {
  if (state == SAFE) {
    setRGB(false, true, false);
  }
  else if (state == WARNING) {
    setRGB(true, true, false);
  }
  else if (state == CRITICAL) {
    setRGB(true, false, false);
  }
  else if (state == LOCKED) {
    setRGB(false, false, true);
  }
}

void beep(unsigned int frequency, unsigned long duration) {
  ledcAttach(BUZZER_PIN, frequency, 8);
  ledcWriteTone(BUZZER_PIN, frequency);

  delay(duration);

  ledcWriteTone(BUZZER_PIN, 0);
  ledcDetach(BUZZER_PIN);
}

void resetCargoState() {
  currentState = SAFE;
  criticalStart = 0;
  lockActive = false;

  applyStateOutput(SAFE);

  beep(1800, 100);
}

void selectCargoMode(int mode) {
  cargoMode = mode;

  Serial.print("Cargo mode: ");
  Serial.println(profiles[cargoMode].name);

  resetCargoState();
}

void checkButtons() {
  static bool lastVaccine = HIGH;
  static bool lastBlood = HIGH;
  static bool lastOrgan = HIGH;

  bool vaccine = digitalRead(BTN_VACCINE);
  bool blood = digitalRead(BTN_BLOOD);
  bool organ = digitalRead(BTN_ORGAN);

  if (lastVaccine == HIGH && vaccine == LOW) {
    selectCargoMode(0);
  }

  if (lastBlood == HIGH && blood == LOW) {
    selectCargoMode(1);
  }

  if (lastOrgan == HIGH && organ == LOW) {
    selectCargoMode(2);
  }

  lastVaccine = vaccine;
  lastBlood = blood;
  lastOrgan = organ;
}

void processState() {
  if (lockActive) {
    currentState = LOCKED;
    applyStateOutput(LOCKED);

    if (millis() - lastBuzzer >= 3000) {
      beep(1000, 250);
      lastBuzzer = millis();
    }

    return;
  }

  ShipState evaluated = evaluateState();

  if (evaluated == CRITICAL) {
    if (currentState != CRITICAL) {
      currentState = CRITICAL;
      criticalStart = millis();
      beep(1200, 200);
    }

    applyStateOutput(CRITICAL);

    if (
      criticalStart != 0 &&
      millis() - criticalStart >=
      profiles[cargoMode].lockMs
    ) {
      lockActive = true;
      currentState = LOCKED;
      applyStateOutput(LOCKED);
      beep(700, 500);
    }

    return;
  }

  if (evaluated == WARNING) {
    currentState = WARNING;
    criticalStart = 0;
    applyStateOutput(WARNING);
    return;
  }

  currentState = SAFE;
  criticalStart = 0;
  applyStateOutput(SAFE);
}

void printStatus() {
  Serial.println();
  Serial.println("========== CRYOGUARD ==========");

  Serial.print("Cargo: ");
  Serial.println(profiles[cargoMode].name);

  Serial.print("Temperature: ");
  Serial.print(trustedTemp, 2);
  Serial.println(" C");

  Serial.print("Humidity: ");
  Serial.print(trustedHum, 2);
  Serial.println(" %");

  Serial.print("DHT healthy: ");
  Serial.print(tempSensorHealthy[0]);
  Serial.print(" ");
  Serial.print(tempSensorHealthy[1]);
  Serial.print(" ");
  Serial.println(tempSensorHealthy[2]);

  Serial.print("MPU Shock: ");
  Serial.println(shockDetected ? "YES" : "NO");

  Serial.print("State: ");

  if (currentState == SAFE) {
    Serial.println("SAFE");
  }
  else if (currentState == WARNING) {
    Serial.println("WARNING");
  }
  else if (currentState == CRITICAL) {
    Serial.println("CRITICAL");
  }
  else {
    Serial.println("LOCKED");
  }

  Serial.println("===============================");
}

void setup() {
  Serial.begin(115200);
  delay(200);
  Serial.println("BOOT OK");

  pinMode(BTN_VACCINE, INPUT_PULLUP);
  pinMode(BTN_BLOOD, INPUT_PULLUP);
  pinMode(BTN_ORGAN, INPUT_PULLUP);

  pinMode(LED_R_PIN, OUTPUT);
  pinMode(LED_G_PIN, OUTPUT);
  pinMode(LED_B_PIN, OUTPUT);

  pinMode(BUZZER_PIN, OUTPUT);
  pinMode(BATT_PIN, INPUT);

  setRGB(false, true, false);

  dht1.begin();
  dht2.begin();
  dht3.begin();

  Wire.begin(SDA_PIN, SCL_PIN);

  if (!mpu.begin(0x68, &Wire)) {
    Serial.println("MPU6050 NOT FOUND");
    while (true) {
      delay(1000);
    }
  }

  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);

  Serial.println("CryoGuard initialized.");
  Serial.print("Cargo: ");
  Serial.println(profiles[cargoMode].name);

  beep(1800, 100);
}

void loop() {
  checkButtons();

  if (millis() - lastSensorRead >= 2000) {
    lastSensorRead = millis();

    readEnvironment();
    shockDetected = detectShock();

    processState();
    printStatus();
  }

  delay(20);
}