//init stuff for servo
#include <Servo.h>
Servo myServo;
int pos = 0;
static int servoPin = 9;

//init stuff for ultrasound
static int trigPin = 2;
static int echoPin = 3;

//init LED stuff for distance
static int greenPin = 4;
static int redPin = 5;

//init for listening to ultrasound thing
long duration;
long distance;
//yucky boilerplate code
void setup() {
  //ultrasound
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);

  //LEDs
  pinMode(greenPin, OUTPUT);
  pinMode(redPin, OUTPUT);

  //seeing things
  Serial.begin(9600);
  
  //boilerplate
  myServo.attach(servoPin);
}

void loop() {
  //ex1_3();
  //ex2_1();
  ex3_2();
}

float measure(){
  digitalWrite(trigPin, 0);
  delayMicroseconds(5);
  digitalWrite(trigPin, 1);
  delayMicroseconds(10);
  digitalWrite(trigPin, 0);

  duration = pulseIn(echoPin, HIGH);
  return (duration * 0.034) / 2;
}

void ex1_3(){
  float distance = measure();
  Serial.print(distance);
  Serial.print(" cm\n");
  delay(100);
}

void ex2_1(){
  float distance = measure();
  Serial.println(distance);
  digitalWrite(redPin, 0);
  digitalWrite(greenPin, 0);
  if (distance < 30.0f){
    digitalWrite(redPin, 1);
    delay(distance);
  }else{
    digitalWrite(greenPin, 1);
    delay(log(distance));
  }
}

void ex3_2(){
  float distance = measure();
  float pos = 0.0f;
  if (10.0f < distance <= 50.0f){
    pos = 4.5 * distance;
    pos = pos - 45;
  }else if (distance <= 10.0f) {
    pos = 0.0f;
  }else{
    pos = 180.0f;
  }

  myServo.write(pos);
  
  Serial.println(distance);
}