#include <Servo.h>

// piny dla sonaru (HC-SR04)
#define TRIG A4
#define ECHO A5

// pin kontroli serwo (musi być PWM)
#define SERVO 3

Servo serwo;

int stops_1 ;
int speeds = 60;
int trap = 0 ;
void setup() {

stops_1 = 0;

  //PRAWE
pinMode(9, OUTPUT);  // ENB
pinMode(4, OUTPUT);  //IN4
pinMode(5, OUTPUT);  //IN3

//LEWE
pinMode(7, OUTPUT); //IN2
pinMode(8, OUTPUT); //IN1
pinMode(11, OUTPUT); //ENA



  pinMode(TRIG, OUTPUT);    // TRIG startuje sonar
  pinMode(ECHO, INPUT);     // ECHO odbiera powracający impuls

  Serial.begin(9600);

  serwo.attach(SERVO);

/* rozejrzyj się w zakresie od 0 stopni (patrz na jedną burtę)
 *  do 180 stopni (patrz na prawą burtę). Wydrukuj na konsoli
 *  kierunek patrzenia i najbliżej widziany obiekt (pojedynczy pomiar)
 */
 for(byte angle = 0; angle < 180; angle+= 20) {
   //lookAndTellDistance(angle);
    delay(500);
  }

/* patrz przed siebie */
  serwo.write(90);



   backward();
     // analogWrite(11, 0);
   //  analogWrite(9, 0);
   //setSpeed(50);


   delay(100);

}

/*
 *     for(byte angle = 0; angle < 180; angle+= 10) {
    lookAndTellDistance(angle);
 */
void loop() {

  /* nic nie rób */

    for(byte angle = 40; angle <90; angle+= 1) {

         lookAndTellDistance(angle);
         if(trap)
         {
              while(true)
                follow(angle);
           break;
         }
    }



    delay(200);





  }

  void follow(byte angle){

      unsigned long tot;      // czas powrotu (time-of-travel)
  unsigned int distance;

  Serial.print("Patrzę w kącie ");
  Serial.print(angle);
  serwo.write(angle);

/* uruchamia sonar (puls 10 ms na `TRIGGER')
 * oczekuje na powrotny sygnał i aktualizuje
 */
  digitalWrite(TRIG, HIGH);
  delay(10);
  digitalWrite(TRIG, LOW);
  tot = pulseIn(ECHO, HIGH);


/* prędkość dźwięku = 340m/s => 1 cm w 29 mikrosekund
 * droga tam i z powrotem, zatem:
 */
  distance = tot/58;


  int  distance_old = distance ;
  delay(200);
    digitalWrite(TRIG, HIGH);
  delay(10);
  digitalWrite(TRIG, LOW);
  tot = pulseIn(ECHO, HIGH);


/* prędkość dźwięku = 340m/s => 1 cm w 29 mikrosekund
 * droga tam i z powrotem, zatem:
 */
  distance = tot/58;

  if(distance > distance_old)
    backward();
  else
  forward();




  }


void lookAndTellDistance(byte angle) {


  unsigned long tot;      // czas powrotu (time-of-travel)
  unsigned int distance;

  Serial.print("Patrzę w kącie ");
  Serial.print(angle);
  serwo.write(angle);

/* uruchamia sonar (puls 10 ms na `TRIGGER')
 * oczekuje na powrotny sygnał i aktualizuje
 */
  digitalWrite(TRIG, HIGH);
  delay(10);
  digitalWrite(TRIG, LOW);
  tot = pulseIn(ECHO, HIGH);

/* prędkość dźwięku = 340m/s => 1 cm w 29 mikrosekund
 * droga tam i z powrotem, zatem:
 */
  distance = tot/58;

byte angle_old = angle;
  if(distance < 500 && distance!=0 ) {
    trap = 1;

       if(distance > 100 && speeds > 10){
        speeds = speeds - 10;
            analogWrite(9, speeds);
           analogWrite(11, speeds);

       }
       else stops();

       //if(distance > 100) {


       // setSpeed(speeds);
     //  }

    // stops();
/*
   for(int i= 0; i<10; i++) {
         angle = angle + 10*i;

         digitalWrite(TRIG, HIGH);
         delay(10);
         digitalWrite(TRIG, LOW);
         tot = pulseIn(ECHO, HIGH);
         distance = tot/58;
         if(distance > 50)
         {
          if(angle < 90) {
            turnleft();
          }
          else
          {
             turnright();
          }

          break;
         }

         angle = angle -10 - 10*i;

          digitalWrite(TRIG, HIGH);
         delay(10);
         digitalWrite(TRIG, LOW);
         tot = pulseIn(ECHO, HIGH);
         distance = tot/58;
         if(distance > 50)
         {
          if(angle < 90) {
            turnright();
          }
          else
          {
             turnleft();
          }

          break;
         }

         angle = angle + 10*i;
    }

    if(angle_old == angle)
    {
      forward();
      delay(100);
      turnleft();
       delay(100);
       backward();

    }
    */

  }
  else {
    speeds = speeds + 10;
     analogWrite(9, speeds);
           analogWrite(11, speeds);
  }

  //if(stops && distance > 100)
    //backward();

  Serial.print(": widzę coś w odległości ");
  Serial.println(distance);
    Serial.println(speeds);
}


void forward() {
//dobrze
  digitalWrite(9,HIGH);
  digitalWrite(4,LOW);
  digitalWrite(5,HIGH);

  digitalWrite(7,HIGH);
  digitalWrite(8,LOW);
  digitalWrite(11,HIGH);

    stops_1 = 0;
}


void turnleft() {

  backward();


    digitalWrite(9,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,LOW);

  delay(200);

    digitalWrite(9,HIGH);
  digitalWrite(4,LOW);
  digitalWrite(5,HIGH);


}

void setSpeed(int speeds){
   analogWrite(11, 0);
   analogWrite(9, 0);

}
void turnright() {

forward();

  digitalWrite(7,LOW);
  digitalWrite(8,LOW);
  digitalWrite(11,LOW);


  delay(900);

  digitalWrite(7,HIGH);
  digitalWrite(8,LOW);
  digitalWrite(11,HIGH);

}


void stops() {
//dobrze
  digitalWrite(9,LOW);
  digitalWrite(4,LOW);
  digitalWrite(5,LOW);

  digitalWrite(7,LOW);
  digitalWrite(8,LOW);
  digitalWrite(11,LOW);

  stops_1 =1;


}
void backward() {


  digitalWrite(9,HIGH);
  digitalWrite(4,HIGH);
  digitalWrite(5,LOW);

  digitalWrite(7,LOW);
  digitalWrite(8,HIGH);
  digitalWrite(11,HIGH);


    stops_1 = 0;

}