#include <Arduino.h>

#define trig_pin 23
#define echo_pin 22
#define red_area_pin 2
#define yellow_area_pin 4
#define green_area_pin 5

#define SOUND_SPEED 0.034

   //Duracion del sonido en regresar
float distanceCm;   // Variable para almacenar la distancia en cm
float distanceInch; // Variable para almacenar la distancia en in



double distance, duration;
double kaldist;

double kalman(double U){
  static const double R = 40;
  static const double H = 1.00;
  static double Q = 10;
  static double P = 0;
  static double U_hat = 0;
  static double K = 0;
  K = P*H/(H*P*H+R);
  U_hat += + K*(U-H*U_hat);
  P = (1-K*H)*P+Q;
  return U_hat;
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  pinMode(trig_pin, OUTPUT);
  pinMode(echo_pin, INPUT);
  pinMode(red_area_pin, OUTPUT);
  pinMode(yellow_area_pin, OUTPUT);
  pinMode(green_area_pin, OUTPUT);
  Serial.println("Ultrasonic Sensor with Filters"); //Filtro EMA, Filtro Kalman


}

void loop() {
  // put your main code here, to run repeatedly:
  digitalWrite(trig_pin, LOW); //Asegurar el apagado del trig
  delayMicroseconds(2);
  digitalWrite(trig_pin, HIGH); //Asegurar el encendido del trig
  delayMicroseconds(10);  

  //duration = pulseIn(echo_pin, HIGH); //Cuanto dura ECHO en HIGH, leer el tiempo de regreso del sonido

  //distanceInch = distanceCm * CM_TO_INCH;


  



//FILTRO kalman
 
  duration = pulseIn(echo_pin, HIGH);
  distance = duration * SOUND_SPEED/2; 
  kaldist = kalman(distance);

  Serial.print("Distance (in cm): ");
  Serial.println(distance);
  Serial.print("Corrected distance (in cm): %f");
  Serial.println(kaldist);



  if (kaldist >260 && kaldist <400){
    digitalWrite(red_area_pin, HIGH);
    digitalWrite(yellow_area_pin, LOW);
    digitalWrite(green_area_pin, LOW);
    Serial.println("Red Zone");
  }

    if (kaldist >130 && kaldist <260){
    digitalWrite(yellow_area_pin, HIGH);
    digitalWrite(green_area_pin, LOW);
    digitalWrite(red_area_pin, LOW);
    Serial.println("Yellow Zone");
  }

    if (kaldist >= 0 && kaldist <=130){
    digitalWrite(green_area_pin, HIGH);
    digitalWrite(yellow_area_pin, LOW);
    digitalWrite(red_area_pin, LOW);
    Serial.println("Green Zone");
  }

  Serial.print("Distance in cm: ");
  Serial.println(distance);
  Serial.print("Distance FILTER: ");
  Serial.println(kaldist);  

  delay(100);
}