/*
  Prova GLOBAL SOLUTIONS - 2o Semestre
  IOT - Mestre Morgantini

  Alunos:
  Bruno Pinheiro RM 556184
  Matheus Gonçalves RM 96957
  Roberta Aquila RM 554455
*/

//Definições das portas usadas para não precisar fica repetindo no código
#define LED1 5
#define LED2 6
#define LED3 9
#define LED4 10
#define LED5 11
#define PIR1 7
#define PIR2 8 
#define LDR A0
#define TRIG 3
#define ECHO 4
#define TRIG2 13
#define ECHO2 12

#include <Wire.h>
#include <MPU6050.h>
MPU6050 mpu;

//Valores iniciais para as variáveis
int analogValue = 0;
int distancia, distancia2;
const int LOW_LIGHT = 5; 
const int MEDIUM_LIGHT = 125;
const int HIGH_LIGHT = 255;

const float GAMMA = 0.7;
const float RL10 = 50;

// Definições para detecção de queda
const float TILT_THRESHOLD = 45.0; // Ângulo de inclinação considerado como queda (em graus)

void setup() {
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(LED3, OUTPUT);
  pinMode(LED4, OUTPUT);
  pinMode(LED5, OUTPUT);
  pinMode(PIR1, INPUT_PULLUP);
  pinMode(PIR2, INPUT_PULLUP);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(TRIG2, OUTPUT);
  pinMode(ECHO2, INPUT);

  Wire.begin();
  mpu.initialize();

  if (!mpu.testConnection()) {
    Serial.println("MPU6050 não encontrado. Verifique as conexões!");
  }

  Serial.begin(4800);
}

void loop() {
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);

  digitalWrite(TRIG2, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG2, LOW);

  distancia = pulseIn(ECHO, HIGH) / 58;
  distancia2 = pulseIn(ECHO2, HIGH) / 58;

  analogValue = analogRead(LDR);

  // Converte o valor analógico em valor lux:
  float voltage = analogValue / 1024. * 5;
  float resistance = 2000 * voltage / (1 - voltage / 5);
  float lux = pow(RL10 * 1e3 * pow(10, GAMMA) / resistance, (1 / GAMMA));
  Serial.println(lux);

  // Se estiver escurecendo a rua
  if (lux < 50) {  
    bool pir_move1 = digitalRead(PIR1); 
    bool pir_move2 = digitalRead(PIR2);
    
    // Define brilho inicial baixo para todos os LEDs
    int lux_led1 = LOW_LIGHT;
    int lux_led2 = LOW_LIGHT;
    int lux_led3 = LOW_LIGHT;
    int lux_led4 = LOW_LIGHT;
    int lux_led5 = LOW_LIGHT;
    
    // Se detectar algum movimento no poste 2
    if (pir_move1) {
      lux_led1 = MEDIUM_LIGHT;  // LED anterior
      lux_led2 = MEDIUM_LIGHT;  // LED do sensor
      lux_led3 = MEDIUM_LIGHT;  // LED posterior

    Serial.println(distancia);
    
    if(distancia < 400){
      lux_led1 = HIGH_LIGHT;  // Aumenta para 100% quando a pessoa esta perto daquele poste
      Serial.println("Pessoa perto do led 1");
    }
    else if (distancia >= 400 && distancia2 < 400){
      lux_led3 = HIGH_LIGHT;  // Aumenta para 100% quando a pessoa esta perto daquele poste
      Serial.println("Pessoa perto do led 3");
    }
    else if (distancia >= 400 && distancia2 >= 400){
      lux_led2 = HIGH_LIGHT;
      Serial.println("Pessoa perto do led 2");
    }
    else {
      Serial.println("Pessoa onipresente");
    }

      delay(5000);// Esperar 5 segundos para voltar para 30%
      Serial.println("Luzes do 1-3 voltando para 30%");
    }
    
    // Se detectar movimento no poste 4
    if (pir_move2) {
      lux_led3 = HIGH_LIGHT;  // LED anterior
      lux_led4 = HIGH_LIGHT;  // LED do sensor
      lux_led5 = HIGH_LIGHT;  // LED posterior

      Serial.println(distancia);
    
      if(distancia < 400){
        lux_led3 = HIGH_LIGHT;  // Aumenta para 100% quando a pessoa esta perto daquele poste
        Serial.println("Pessoa perto do led 3");
      }
      else if (distancia >= 400 && distancia2 < 400){
        lux_led5 = HIGH_LIGHT;  // Aumenta para 100% quando a pessoa esta perto daquele poste
        Serial.println("Pessoa perto do led 5");
      }
      else if (distancia >= 400 && distancia2 >= 400){
        lux_led4 = HIGH_LIGHT;
        Serial.println("Pessoa perto do led 4");
      }
      else {
        Serial.println("Pessoa onipresente");
      }

      delay(5000); // Esperar 5 segundos para voltar para 30%
      Serial.println("Luzes do 3-5 voltando para 30%");
    }
    
    // Aplica os valores HIGH ou LOW nos LEDs, dependendo da lógica anterior
    analogWrite(LED1, lux_led1);
    analogWrite(LED2, lux_led2);
    analogWrite(LED3, lux_led3);
    analogWrite(LED4, lux_led4);
    analogWrite(LED5, lux_led5);
    
  } else {  // Se estiver amanhecendo
    // Apaga todos os LEDs
    analogWrite(LED1, 0);
    analogWrite(LED2, 0);
    analogWrite(LED3, 0);
    analogWrite(LED4, 0);
    analogWrite(LED5, 0);
  }

  // Verificação de queda do poste
  verificarQuedaPoste();

  delay(500);  // Delay 
}

void verificarQuedaPoste() {
  int16_t ax, ay, az;
  int16_t gx, gy, gz;
  mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  float anguloX = atan2(gy, gz) * 180 / PI;
  float anguloY = atan2(gx, gz) * 180 / PI;
  
  // Usa o maior ângulo absoluto como inclinação geral
  float anguloInclinacao = max(abs(anguloX), abs(anguloY));

  if (anguloInclinacao > TILT_THRESHOLD) {
    Serial.println("ALERTA: Poste caiu!");
  } else {
    Serial.println("Poste na posição normal.");
  }

  // Imprime o ângulo de inclinação para monitoramento
  Serial.print("Ângulo de inclinação: ");
  Serial.println(anguloInclinacao);
}