#include <Wire.h>
#include "I2Cdev.h"
#include "MPU6050.h"
#include "DHT.h"
#include "NMEA.h"
#include <WiFi.h>
#include <PubSubClient.h>
#include <Arduino.h>
#include <LiquidCrystal_I2C.h>
#define DHTPIN 4
#define DHTTYPE DHT22

// MPU-6050
MPU6050 mpu;
int16_t ax, ay, az;
int16_t gx, gy, gz;

// DHT22
DHT dht(DHTPIN, DHTTYPE);

// GPS
#define LEN(arr) ((int)(sizeof(arr) / sizeof(arr)[0]))
union {
  char bytes[4];
  float valor;
} velocidadeGPS;

float latitude;
float longitude;
NMEA gps(GPRMC);  // Creates a GPS data connection with sentence type GPRMC

#define lcd_ADDR 0x27
#define lcd_COLUMNS 16
#define lcd_ROWS 2

LiquidCrystal_I2C lcd(lcd_ADDR, lcd_COLUMNS, lcd_ROWS);

#define PULSE_PIN 35

//#define MQTT_SERVER "broker.emqx.io"
//#define MQTT_PORT 1883

//const char *ssid = "Wokwi-GUEST";
//const char *password = "";
#define redLEDPin 12
#define greenLEDPin 14

//#define MQTT_TOPIC_HR "/heartRate"


//WiFiClient espClient;
//PubSubClient client(espClient);

int minHeartRate = 60;
int maxHeartRate = 100;
void setup() {
  Serial.begin(115200);
  
  // Initialize MPU-6050
  Wire.begin();
  mpu.initialize();
  if (!mpu.testConnection()) {
    Serial.println("MPU6050 connection failed");
    while (1);
  }
  
  // Initialize DHT22
  dht.begin();

  // Initialize GPS
  Serial2.begin(9600);  // Serial2 is connected to the GPS chip
  Serial.println("Data received from GPS:");

  lcd.init();
  lcd.backlight();
  Wire.begin(23, 22);
  Serial.begin(115200);
  lcd.print("Heart Monitor");
  pinMode(redLEDPin, OUTPUT);
  pinMode(greenLEDPin, OUTPUT);
}

void loop() {
  // Read MPU-6050 data
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  Serial.print("Accel: ");
  Serial.print(ax); Serial.print(" ");
  Serial.print(ay); Serial.print(" ");
  Serial.print(az); Serial.print("\t");
  Serial.print("Gyro: ");
  Serial.print(gx); Serial.print(" ");
  Serial.print(gy); Serial.print(" ");
  Serial.print(gz); Serial.println();

  // Read DHT22 data
  float humidity = dht.readHumidity();
  if (isnan(humidity)) {
    Serial.println("Failed to read from DHT sensor!");
  } else {
    Serial.print("Humidity: ");
    Serial.print(humidity);
    Serial.println(" %");
  }

  // Read and process GPS data
  while (Serial2.available()) {             // Waits for serial port data

    char serialData = Serial2.read();       // Receives data from GPS serial port
    Serial.print(serialData);


    if (gps.decode(serialData)) {           // Checks if the GPS sentence is valid
      if (gps.gprmc_status() == 'A') {      // Checks if GPS status is 'A'

        velocidadeGPS.valor = gps.gprmc_speed(KMPH);        // Receives GPS speed in km/h

      } else {
        velocidadeGPS.valor = 0;
      }

      latitude  = gps.gprmc_latitude();
      longitude = gps.gprmc_longitude();

      // Add line break
      Serial.println();
      Serial.println();

      // Show Latitude
      Serial.print(" Latitude: ");
      Serial.println(latitude, 8);

      // Show Longitude
      Serial.print("Longitude: ");
      Serial.println(longitude, 8);

      // Show Speed ​​in km/h
      Serial.print("    Speed: ");
      Serial.print(velocidadeGPS.valor);
      Serial.println(" Km/h");

      // Converts Geographic Coordinates to Cartesian Plane
      convertCoordinatesToCartesian(latitude, longitude);
    }
  }
  int16_t pulseValue = analogRead(PULSE_PIN);
  
  // Convert pulseValue to voltage
  float voltage = pulseValue * (5 / 4095.0);
  
  // calculate heartRate from voltage
  int heartRate = (voltage / 3.3) * 675;

  
  if (heartRate < minHeartRate){
    lcd.clear();
    lcd.print("Heart rate below ");
    lcd.setCursor(0, 1);
    lcd.print("minimum: ");
    lcd.println(heartRate);
    lcd.setCursor(13,1);
    lcd.print("b/m");
    digitalWrite(redLEDPin, HIGH); // Turn on LED
    delay(20); // Wait for half of the blink interval
    digitalWrite(redLEDPin, LOW); // Turn off LED
    delay(20); // Wait for the other half of the blink interval

  }
  else if (heartRate > maxHeartRate){
    lcd.clear();
    lcd.print("Heart rate above ");
    lcd.setCursor(0, 1);
    lcd.print("maximum: ");
    lcd.println(heartRate);
    lcd.setCursor(13,1);
    lcd.print("b/m");
    digitalWrite(redLEDPin, HIGH); // Turn on LED
    delay(20); // Wait for half of the blink interval
    digitalWrite(redLEDPin, LOW); // Turn off LED
    delay(20); // Wait for the other half of the blink interval

    
  }else{
    lcd.clear();
    
    lcd.println("Heart Monitor");
    lcd.setCursor(0, 1);
    lcd.print("Heart rate:");
    lcd.println(heartRate);
     lcd.setCursor(13,1);
    lcd.println("b/m");
    digitalWrite(greenLEDPin, HIGH); // Turn on LED
    delay(100); // Wait for half of the blink interval
    digitalWrite(greenLEDPin, LOW); // Turn off LED
    delay(100); // Wait for the other half of the blink interval

  }

  delay(1000);
}

void convertCoordinatesToCartesian(float latitude, float longitude) {
  float latRadius = latitude  * (PI) / 180;  // Convert from Degrees to Radians
  float lonRadius = longitude * (PI) / 180;

  int earthRadius = 6371; // Radius in km

  float posX = earthRadius * cos(latRadius) * cos(lonRadius);
  float posY = earthRadius * cos(latRadius) * sin(lonRadius);

  Serial.print("        X: ");  Serial.println(posX);
  Serial.print("        Y: ");  Serial.println(posY);
}