// include the distance sensor library
#include "Ultrasonic.h"

// Define the pins
const int ledPins[4] = {11, 10, 6, 5}; // LED pins
const int speakerPin = 9; // speaker pin
const int trigPin = 12; // "trigger" pin for the distance sensor
const int echoPin = 13; // "echo" pin for the distance sensor

// create an instance of the distance sensor
Ultrasonic ultrasonic( trigPin, echoPin );

// array to store the last 5 distance readings
// it is used to smooth out any sudden jumps
int histValues[5] = {201, 201, 201, 201, 201};
// next position in the value array above
int indValue = 0;

// Define some operational timers
unsigned long sensorNext = 0; // timer to schedule sensor polling
unsigned long servoNext = 0; // timer to schedule servo movement

// Initial servo positions (0 is 90 degrees)
int servoPos = 0;
// Initial servo direction (1 = forward from 90 degrees, -1 = backward)
int servoDir = 1;

// function to set a particular servo on pololu (0-7) to an angle
// the angle from 0 to 180 is loosely transformed into absolute servo position
void setServoPos (unsigned int servoNum, unsigned int angle) {

   // command array (6 bytes)
   unsigned char command[6];
   // remap the angle to adjusted pololu operational range
   // even though it should be 500-5500, it works better with 500-5000
   unsigned int tempAngle = map (angle, 0, 180, 500, 5000);

   // calculate the "high" position byte
   // by zeroing out the 7 lest significant bits
   // and shifting the other bits right by 7 bits
   unsigned int posHigh = (tempAngle&0x1F80)>>7;
   // calculate the "low" position byte
   // by zeroing out everything but the 7 least significant bits
   unsigned int posLow = tempAngle&0x7F;

   command[0] = 0x80; // start byte
   command[1] = 0x01; // device ID
   command[2] = 0x04; // set absolute position command
   command[3] = servoNum; // servo number
   command[4] = posHigh; // position (high byte)
   command[5] = posLow; // position (low byte)

   // write the command to pololu via serial port
   Serial.write(command,6);

   // wait for pololu to process
   delay(10);

}

// function to set the servo speed based on the current distance
// expects preset "servo position" (1-4) as input to map to speed
// speed is capped at 70 to avoid wild swings and epic destruction
void setServoSpd (unsigned int servoNum, unsigned int servoPos) {
   // command array (5 bytes)
   unsigned char command[5];
   // remap the position value (1-4) to speed (1-70)
   unsigned int tempSpeed = map (servoPos, 1, 4, 1, 70);

   command[0] = 0x80; // start byte
   command[1] = 0x01; // device ID
   command[2] = 0x01; // set speed command
   command[3] = servoNum; // servo number
   command[4] = tempSpeed; // speed

   // write the command to pololu via serial port
   Serial.write(command,5);

   // wait for pololu to process
   delay(10);
}

// initialize the installation
void setup()
{
   // mute the speaker
   noTone(speakerPin);
   // initialize serial port to pololu (38400 baud)
   Serial.begin(38400);
   // let pololu sync
   delay(200);

   // set servo speed to max (position 4)
   setServoSpd(0,4);
   delay(500);
   // move servo 0 to angle 90
   setServoPos(0,90);
   // schedule the sensor to be polled in 200 ms
   sensorNext = millis() + 200;   
}

// main program
void loop()
{
  int i;
   
  // check if the sensing routing is scheduled to run based on the timer
  // this routine fills out the historical distance valus array, adjusts
  // the current speed and position of the servo, and lights the LEDs
  if (sensorNext <= millis()) {
    // read the distance from sensor
    int distanceValue = ultrasonic.Ranging(CM);
    
    // if the distance is over 200cm (2m), we need to turn off
    if (distanceValue >= 200)
      distanceValue = 201; // 201 here is "infinity"
    
    // add the currently read distance to the next slot in historical values array
    histValues[indValue] = distanceValue;
    // increment array indes
    indValue++;
    // if we already have 5 values, wrap around to 0    
    if (indValue == 5)
      indValue = 0;

    // figure out the minimum distance from the historical values array
    // the initial setting is 201 (infinity)
    int valMin = 201;
    
    // step through the array and find the lower value
    // we take the lowest value within past 5 seconds as the distance
    for (i = 0; i < 5; i++) {
      if (histValues[i] < valMin)
        valMin = histValues[i];
    }
    
    // need to make notifications only if one of the past measure distances is under 201
    if (valMin != 201) {
      // remap the read distance (0-200) to speaker tone (the closer, the higher)
      distanceValue = map(valMin, 0, 200, 3700, 10);
      // set the speaker to output the tone
      tone(speakerPin, distanceValue);
      // remap the read distance (0-200) to a preset postion for the swing (4-1)
      // the closer the object is, the higher is the speed setting and angle
      servoPos = map (valMin, 0, 200, 4, 1);
      // set the LEDs to variable brightness based on the distance
      for (i = 0; i < 4; i++)
        // the closer the object is, the brighter the LEDs
        analogWrite(ledPins[i], map (valMin, 0, 200, 255, 1));
      // if servo movement is not scheduled, schedule it in 10ms
      if (servoNext == 0)
        servoNext = millis() + 10;
    // if all the historical distances are 201 (infinity), we should stop activity
    } else {
      // move the swing to neutral position (90 degrees)
      servoPos = 0;
      // mute the speaker
      noTone(speakerPin);
      // turn off all LEDs
      for (i = 0; i < 4; i++)
        analogWrite(ledPins[i],0);
    }
    // schedule the next sensor poll in a second
    sensorNext = millis() + 1000;
  }
  
  // check if it's time to move the swing 
  if (servoNext <= millis()) {
    // if the requested position is 0, move to neutral (90 degrees)
    if (servoPos == 0) {      
      setServoPos(0, 90);
      // reset the direction to forward
      servoDir = 1;  
    // otherwise, start swinging
    } else {      
      // set the speed based on position (1-4, 4 being the fastest)
      setServoSpd(0,servoPos);
      // set the servo position to move the swing
      // each position(1-4) increases the movement angle from 90 degrees by 5 degrees
      // so, with position 1, the angles for the swing are 85-95
      // with position 4, the angles for the swing are 70-110
      // this should be adjusted with care to avoid damage to the swing
      setServoPos(0,90+5*servoDir*servoPos);
      // after moving the swing, reverse the future direction to backward
      servoDir = -1*servoDir;
      // schedule the next servo movement
      // 0.7 second delay is allowed per swing at speed = 4
      // 3.5 second delay is allowed per swing at speed = 1
      // adjust the delay with care to avoid damage
      servoNext = millis()+700*(5-servoPos);
    }
  } 
}