#include <Wire.h>
#include <WiFi.h>
#include <WiFiClient.h>
#include <SoftwareSerial.h>

// defining gsm attachment using uart 
#define MYPORT_TX 12
#define MYPORT_RX 13



 // Standard hardware serial port

const int MPU_addr = 0x68; // I2C address of the MPU-6050
int16_t AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ;
float ax = 0, ay = 0, az = 0, gx = 0, gy = 0, gz = 0;

boolean fall = false; // stores if a fall has occurred

boolean trigger1 = false; // stores if the first trigger (lower threshold) has occurred
boolean trigger2 = false; // stores if the second trigger (upper threshold) has occurred
boolean trigger3 = false; // stores if the third trigger (orientation change) has occurred

byte trigger1count = 0; // stores the counts past since trigger 1 was set true
byte trigger2count = 0; // stores the counts past since trigger 2 was set true
byte trigger3count = 0; // stores the counts past since trigger 3 was set true

int angleChange = 0;

/* Fill in information from Blynk Device Info here */
#define BLYNK_TEMPLATE_ID "TMPL2RksY9Ep0"
#define BLYNK_TEMPLATE_NAME "Fall detector"
#define BLYNK_AUTH_TOKEN "-OzW1ZNhPnt9LyK-VeGVd0jYizvWVq90"

// we include the below line after specifing the above credentials.....
#include <BlynkSimpleEsp32.h>

#define BLYNK_PRINT Serial

const char *ssid = "wokwi-Guest"; // Enter your Wi-Fi Name
const char *pass = ""; // Enter your Wi-Fi Password
const String nurseNo = "+255712345678";
int Amp= 0;

BlynkTimer timer;
EspSoftwareSerial::UART GSM;

void myTimer() 
{
  Serial.println("reading data from mpu....");
  mpu_read();
  ax = (AcX - 2050) / 16384.00;
  ay = (AcY - 77) / 16384.00;
  az = (AcZ - 1947) / 16384.00;
  gx = (GyX + 270) / 131.07;
  gy = (GyY - 351) / 131.07;
  gz = (GyZ + 136) / 131.07;

  // calculating Amplitude vector for 3 axis
  float Raw_Amp = pow(pow(ax, 2) + pow(ay, 2) + pow(az, 2), 0.5);
  Amp = Raw_Amp * 10;

  Serial.println(Amp);

  if (Amp <= 2 && trigger2 == false) { // if AM breaks lower threshold (0.4g)
    trigger1 = true;
    Serial.println("TRIGGER 1 ACTIVATED");
  }

  if (trigger1 == true) {
    trigger1count++;

    if (Amp >= 12) { // if AM breaks upper threshold (3g)
      trigger2 = true;
      Serial.println("TRIGGER 2 ACTIVATED");
      trigger1 = false; trigger1count = 0;
    }
  }

  if (trigger2 == true) {
    trigger2count++;
    angleChange = pow(pow(gx, 2) + pow(gy, 2) + pow(gz, 2), 0.5);
    Serial.println(angleChange);

    if (angleChange >= 30 && angleChange <= 400) { // if orientation changes by between 80-100 degrees
      trigger3 = true; trigger2 = false; trigger2count = 0;
      Serial.println(angleChange);
      Serial.println("TRIGGER 3 ACTIVATED");
    }
  }

  if (trigger3 == true) {
    trigger3count++;

    if (trigger3count >= 10) {
      angleChange = pow(pow(gx, 2) + pow(gy, 2) + pow(gz, 2), 0.5);

      Serial.println(angleChange);

      if ((angleChange >= 0) && (angleChange <= 10)) { // if orientation changes remain between 0-10 degrees
        fall = true; trigger3 = false; trigger3count = 0;
        Serial.println(angleChange);
      }
      else {
        trigger3 = false; trigger3count = 0;
        Serial.println("TRIGGER 3 DEACTIVATED");
      }
    }
  }

  if (fall == true) { // in the event of a fall detection
    Serial.println("FALL DETECTED");
    Blynk.virtualWrite(V3, Amp);
    sendSms(nurseNo, "Patient number 0 has fallen.Please reach out immediately!...");
    fall = false;
  }

  delay(100);
  // This function describes what will happen with each timer tick
  // e.g. writing sensor value to datastream V5
  Blynk.virtualWrite(V4, Amp );  
}


void setup() {
  Serial.begin(115200);
// setting up GSM 
  GSM.begin(9600, SWSERIAL_8N1, MYPORT_RX, MYPORT_TX, false);
  if (!GSM) { // If the object did not initialize, then its configuration is invalid
  
  Serial.println("Invalid EspSoftwareSerial pin configuration, check config"); 
  while (1) { // Don't continue with invalid configuration
    delay (1000);
  }

  }
  sendATCommand("AT+IPR=9600");  // Set the SMS mode to text
  delay(1000);
  sendATCommand("AT+CSCA?");  // Set the SMS mode to text
  delay(1000);
  // vTaskDelay(100 / portTICK_PERIOD_MS);
  sendATCommand("AT+CMEE=1");  // Set the SMS mode to text
  delay(1000);
  // vTaskDelay(1000 / portTICK_PERIOD_MS);

  Wire.begin();
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x6B);  // PWR_MGMT_1 register
  Wire.write(0);     // set to zero (wakes up the MPU-6050)
  Wire.endTransmission(true);
  Serial.println("Wrote to IMU");
  Serial.println("Connecting to ");
  Serial.println(ssid);
  // WiFi.begin("Wokwi-GUEST", "", 6);
  // while (WiFi.status() != WL_CONNECTED) {
  //   delay(500);
  //   Serial.print(".");
  // }
  Serial.println("");
  Serial.println("WiFi connected");

  Blynk.begin(BLYNK_AUTH_TOKEN, "Wokwi-GUEST", "");
    // Setup a timer function to be called every second
  timer.setInterval(1000, myTimer);

}

void loop() {
  Blynk.run();
  timer.run();
}


void mpu_read() {
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x3B);  // starting with register 0x3B (ACCEL_XOUT_H)
  Wire.endTransmission(false);
 Wire.requestFrom(MPU_addr, 14); // request a total of 14 registers without sending a stop signal


  AcX = Wire.read() << 8 | Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
  AcY = Wire.read() << 8 | Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
  AcZ = Wire.read() << 8 | Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
  Tmp = Wire.read() << 8 | Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
  GyX = Wire.read() << 8 | Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
  GyY = Wire.read() << 8 | Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
  GyZ = Wire.read() << 8 | Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)
}


void sendATCommand(String command) {
  GSM.println(command);
  delay(500);

  while (GSM.available()) {
    Serial.println(GSM.readStringUntil('\n'));
  }
  Serial.println();
}

void sendSms(String number, String message) {
  sendATCommand("AT+CMGF=1");  // Set the SMS mode to text
  delay(1000);
  sendATCommand("AT+CMGS=\"" + number + "\"");  // Replace with the recipient's phone number
  delay(1000);
  GSM.print(message);  // Replace with your desired message
  delay(1000);
  GSM.write(26);  // ASCII code of Ctrl+Z (end of message)
  delay(1000);
}