/*
  XIAO ESP32-C3 Recovery System

  Connections:
  MPU6050
    SDA -> D1
    SCL -> D0
    VCC -> 3.3V
    GND -> GND

  Buzzer
    + -> D10
    - -> GND

  Servo
    Signal -> GPIO21

  Battery Monitor
    Divider Output -> D3
    Divider Ratio = 1:1
*/

#include <Wire.h>
#include <MPU6050.h>
#include <ESP32Servo.h>

MPU6050 mpu;
Servo parachuteServo;

// Pins
const int BUZZER_PIN = D10;
const int SERVO_PIN = 21;
const int BATTERY_PIN = D2;

// Servo positions
const int CLOSED_POS = 90;
const int OPEN_POS = 180;

// Free-fall settings
const float FREE_FALL_THRESHOLD = 0.30;   // g
const unsigned long FREE_FALL_TIME = 30; // ms

bool freeFallStarted = false;
bool parachuteDeployed = false;

unsigned long freeFallStartTime = 0;
unsigned long lastBatteryRead = 0;

// -------------------------
// Battery Functions
// -------------------------
float getBatteryVoltage()
{
  int adc = analogRead(BATTERY_PIN);

  // ESP32-C3 ADC
  float adcVoltage = (adc / 4096.0) * 3.3;

  // 1:1 divider
  float batteryVoltage = adcVoltage * 2.0;

  return batteryVoltage;
}

int getBatteryPercent(float voltage)
{
  if (voltage >= 4.20)
    return 100;

  if (voltage <= 3.20)
    return 0;

  return (int)((voltage - 3.20) * 100.0 / (4.20 - 3.20));
  //return voltage;
}

// -------------------------
// Deploy Parachute
// -------------------------
void deployParachute()
{
  if (!parachuteDeployed)
  {
    parachuteServo.write(OPEN_POS);
    parachuteDeployed = true;

    Serial.println("================================");
    Serial.println("FREE FALL DETECTED!");
    Serial.println("PARACHUTE DEPLOYED!");
    Serial.println("================================");
  }
}

void setup()
{
  Serial.begin(115200);
  delay(1000);

  Serial.println("Starting Recovery System...");

  // I2C
  Wire.begin(D1, D0);

  // MPU6050
  mpu.initialize();

  if (!mpu.testConnection())
  {
    Serial.println("MPU6050 connection FAILED!");
    while (1)
    {
      delay(100);
    }
  }

  Serial.println("MPU6050 connected.");

  // Battery ADC
  analogReadResolution(12);

  // Buzzer
  pinMode(BUZZER_PIN, OUTPUT);
  noTone(BUZZER_PIN);

  // Servo
  parachuteServo.attach(SERVO_PIN);
  parachuteServo.write(CLOSED_POS);

  Serial.println("System Ready.");
}

void loop()
{
  // -------------------------
  // Read MPU6050
  // -------------------------
  int16_t ax, ay, az;

  mpu.getAcceleration(&ax, &ay, &az);

  float ax_g = ax / 16384.0;
  float ay_g = ay / 16384.0;
  float az_g = az / 16384.0;

  float accelMagnitude =
      sqrt(ax_g * ax_g +
           ay_g * ay_g +
           az_g * az_g);

  // -------------------------
  // Free Fall Detection
  // -------------------------
  if (!parachuteDeployed)
  {
    if (accelMagnitude < FREE_FALL_THRESHOLD)
    {
      if (!freeFallStarted)
      {
        freeFallStarted = true;
        freeFallStartTime = millis();

        Serial.println("Possible free fall...");
      }

      if (millis() - freeFallStartTime >= FREE_FALL_TIME)
      {
        deployParachute();
      }
    }
    else
    {
      freeFallStarted = false;
    }
  }

  // -------------------------
  // Alarm after deployment
  // -------------------------
  if (parachuteDeployed)
  {
    static unsigned long lastToneChange = 0;
    static bool highTone = false;

    if (millis() - lastToneChange > 200)
    {
      lastToneChange = millis();

      highTone = !highTone;

      if (highTone)
      {
        tone(BUZZER_PIN, 3000);
      }
      else
      {
        tone(BUZZER_PIN, 1500);
      }
    }
  }

  // -------------------------
  // Battery Status
  // -------------------------
  if (millis() - lastBatteryRead > 1000)
  {
    lastBatteryRead = millis();

    float batteryVoltage = getBatteryVoltage();
    int batteryPercent = getBatteryPercent(batteryVoltage);

    Serial.print("Battery: ");
    Serial.print(batteryVoltage, 2);
    Serial.print(" V | ");

    Serial.print(batteryPercent);
    Serial.print("% | ");

    Serial.print("Accel Magnitude: ");
    Serial.println(accelMagnitude, 3);
  }

  delay(10);
}