#include <CodeRacer.h>

//----- settings for the ultrasonic sensor -----
#define US_STOP_ABSTAND_CM 20     // if distance goes below that - stop the racer

//----- variables we need 
unsigned long distance_cm = 0; 

//---- construct the coderacer object
CodeRacer coderacer;

//---- set up code - executed ones
void setup() {
    // start serial monitor
    Serial.begin(115200);
    // initialize the coderacer
    coderacer.begin();
    // enable fun stuff
    coderacer.coderacer_fun_enabled = true;
    // look to the left, to the right and to center... :-)
    coderacer.servo_set_to_left();
    delay(100);
    coderacer.servo_set_to_right();
    delay(100);    
    coderacer.servo_set_to_center(); 
    delay(100);
}

//---- 'endless' loop  
void loop() {

  // check if the racer was started (button was toggled to coderacer active state 
  if(true == coderacer.start_stop()){

    Serial.print("Speed of right side drive: ");
    Serial.println(coderacer.drive_right_speed());
    Serial.print("Speed of left side drive: ");
    Serial.println(coderacer.drive_left_speed());

    // measure the distance - at the position of the servo 
    distance_cm = coderacer.usonic_measure_cm();  
         
    coderacer.start_stop_at_min_distance(US_STOP_ABSTAND_CM);
    while(!coderacer.stopped_at_min_distance()){

      Serial.print("Distanc in cm: ");
      Serial.println(distance_cm);    
      
      if(distance_cm > 50){
        coderacer.drive_forward();
        coderacer.servo_sweep();
      }
      else if(distance_cm > 40){
        coderacer.turn_right();        
      }
      else if(distance_cm > 30){
        coderacer.turn_left();
      }
      else { 

        coderacer.drive_backward();
      }

      // measure the distance - at the position of the servo 
      distance_cm = coderacer.usonic_measure_cm();  
      
    }
    
    Serial.println("***** STOPPED ***** ");
    Serial.print("Measured stop distanc of cm: ");
    Serial.println(distance_cm);    
    Serial.print("Measured at servo position of: ");
    Serial.println(coderacer.servo_position());

    coderacer.set_inactive();
    
  }
  
}