#include <Ultrasonic.h>
#include <Servo.h>

#define DEBUG         false

#define ECHO_PIN      5
#define TRIGGER_PIN   6
#define BUTTON_PIN    7
#define MOTOR_PIN    10
#define LED_PIN      13

#define MIN_MOTOR    90
#define MAX_MOTOR   110 // maximum is actually around 180! be careful when setting it to higher speeds!

#define MIN_DISTANCE  0
#define MAX_DISTANCE 10

#define SERIAL_BAUD  57600

Ultrasonic _ultrasonic(TRIGGER_PIN, ECHO_PIN);
Servo  _motor;
boolean _stateOn;
boolean _stateLock;

void setup() {
  Serial.begin(SERIAL_BAUD);
  pinMode(BUTTON_PIN, INPUT);
  pinMode(LED_PIN, OUTPUT);
  
  // turn on the internal pull-up resistor
  digitalWrite(BUTTON_PIN, HIGH);  
  
  _motor.attach(MOTOR_PIN);
  _motor.write(MIN_MOTOR);
  _stateOn = false;
  _stateLock = false;
  if (DEBUG) Serial.println("initializing...");
}

void loop() {
  handleButtonState();
  if (_stateOn) {
     int motorPower = getMotorPower(); 
     _motor.write(motorPower);  
  }
}



void handleButtonState() {
  boolean pressed = (digitalRead(BUTTON_PIN) == LOW);
  if (pressed && !_stateLock) {
      _stateOn = !_stateOn;
      _stateLock = true;
      if (_stateOn) {
         digitalWrite(LED_PIN, HIGH); 
      } else {
        digitalWrite(LED_PIN, LOW);
      }
      
      if (DEBUG) Serial.println("changing state");
  } else if (!pressed) {
    _stateLock = false;
  }
}


int getMotorPower() {
   long microsec = _ultrasonic.timing(); 
   float cmMsec = _ultrasonic.convert(microsec, Ultrasonic::CM);
   
   if (DEBUG) {  
     Serial.print("CM: ");
     Serial.println(cmMsec);
   }
   
   // normalize value
   if (cmMsec == 0) {
     cmMsec = MAX_DISTANCE;
   }
   
   if (cmMsec > MAX_DISTANCE) {
     cmMsec = MAX_DISTANCE;
   }
   
   cmMsec = MAX_DISTANCE - cmMsec;
   int power = map((int) cmMsec, MIN_DISTANCE, MAX_DISTANCE, MIN_MOTOR, MAX_MOTOR);   
   return power;
}