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

#define TRIG 13
#define ECHO 12
#define Rango 2
#define Rder_1 6
#define Rder_2 5
#define Rizq_1 4
#define Rizq_2 3
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;
int dist_max=0;
int ang_max=0;
int rangoCritico=100;
Servo servo;

void setup() {
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT);
  Serial.begin(9600);
  servo.attach(7);
  pinMode(Rder_1, OUTPUT);
  pinMode(Rder_2, OUTPUT);
  pinMode(Rizq_1, OUTPUT);
  pinMode(Rizq_2, OUTPUT);
  
}

void loop() {
  servo.write(90);
  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<rangoCritico){
    buscasNuevaDirec();
    if (ang_max<90){
      giroIzq();
    } else {
      giroDer();
    }
  }
  Acelero();
  
   
}

void giroDer(){
  digitalWrite(Rder_1, LOW);
  digitalWrite(Rder_2, HIGH);
  digitalWrite(Rizq_1, HIGH);
  digitalWrite(Rizq_2, LOW);
  delay(2000);
}

void giroIzq(){
  digitalWrite(Rder_1, HIGH);
  digitalWrite(Rder_2, LOW);
  digitalWrite(Rizq_1, LOW);
  digitalWrite(Rizq_2, HIGH);
  delay(2000);
}

void Acelero(){
  digitalWrite(Rder_1, HIGH);
  digitalWrite(Rder_2, LOW);
  digitalWrite(Rizq_1, HIGH);
  digitalWrite(Rizq_2, LOW);
}

void buscasNuevaDirec(){
  for(int i=ang_mini;i<ang_maxi;i+=3){  
      servo.write(i);
      delay(30);
      calculate(&i,&obj_ang,&dist_max,&ang_max);
    }
}

void calculate(int* i, float* obj_ang, int* dist_max, int* ang_max){
  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;
  Serial.print(*i);
  Serial.print("===");
  Serial.println(distance);

  if (*i!=180){
    if (distance > *dist_max){
      *dist_max=distance;
      *ang_max=*i;
    }
  } else {
    Serial.print("Distancia Maxima : ");
    Serial.println(*dist_max);
    Serial.print("Angulo maximo : ");
    Serial.println(*ang_max);
  }
}