#include <Wire.h>
#include <Servo.h>
#include <math.h>

#define TRIG 13
#define ECHO 12
#define Rango 2
float obj_ang=-1.0;
bool primera = false;
int mini = 0;
int maxi = 0;
int ang_mini = 0 ;
int ang_maxi = 180;
int distanciaRef;
int suma=0;
int count=0;
Servo servo;
Servo servo2;

void setup() {
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  Serial.begin(9600);
  servo.attach(7);
  servo2.attach(4);
}

void loop() {

    for(int i=ang_mini;i<ang_maxi;i++){  
      servo.write(i);
      delay(30);
      calculate(&i,&obj_ang);
      if (obj_ang!=-1){
        servo2.write(obj_ang);
      }

    }
    for(int i=ang_maxi;i>ang_mini;i--){  
      servo.write(i);
      delay(30);
      calculate(&i,&obj_ang);
      if (obj_ang!=-1){
        servo2.write(obj_ang);
      }
 
    }
   
}

void calculate(int* i, float* obj_ang){
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  delayMicroseconds(200);

  long duration = pulseIn(ECHO, HIGH);
  //convertir el tiempo a distancia
  float distance = (duration * 0.034) / 2;
  if (distance>30 && distance<190){
    //*obj_ang=*i;
    Serial.println("Objeto detectado");
    if (!primera){
      mini=*i;
      distanciaRef = distance;
      Serial.print("primera lectura");
      Serial.print(mini);
      suma=*i;
      count=0;
      primera = true;
    }
    else {
      if (abs(distance-distanciaRef)<=Rango){
      suma+=*i;
      count++;
      Serial.print("ultima");
      } else {
        primera=false;
        Serial.println("Termino");
        Serial.println(suma);
        Serial.println(count);
        float thetaRadar = radians((suma/count));
    
        // Calcular el ángulo corregido
        // L en cm
        int L = 5;
        *obj_ang = degrees(atan2(5 + distance * sin(thetaRadar), distance * cos(thetaRadar)));
        Serial.println(*obj_ang);
        }
    }

    

  } else {
        primera=false;
        Serial.println("Termino");
        Serial.println(suma);
        Serial.println(count);
        float thetaRadar = radians((suma/count));
    
        // Calcular el ángulo corregido
        // L en cm
        int L = 5;
        *obj_ang = degrees(atan2(5 + distance * sin(thetaRadar), distance * cos(thetaRadar)));
        Serial.println(*obj_ang);
        }
 
}