// ----------LIBRARIES--------------
#include <Servo.h>

// --------CONSTANTS (won't change)---------------
const int led_A_Pin = 12;				// LED pin number
const int servoPin = 5;					// the pin number for the servo signal

const int led_A_Interval = 200;

const int blinkDuration = 200;			// number of millisecs that Led's are on - all three leds use this

const int servoMinDegrees = 5;			// the limits to servo movement
const int servoMaxDegrees = 150;


//------------ VARIABLES (will change)---------------------
byte led_A_State = LOW;					//   LOW = off

Servo myservo;							// create servo object to control a servo 

int servoPosition = 90;					// the current angle of the servo - starting at 90.
int servoSlowInterval = 80;				// millisecs between servo moves
int servoFastInterval = 10;
int servoInterval = servoSlowInterval;	// initial millisecs between servo moves
int servoDegrees = 2;					// amount servo moves at each step 
										//    will be changed to negative value for movement in the other direction

unsigned long currentMillis = 0;   		// stores the value of millis() in each iteration of loop()
unsigned long previousLed_A_Millis = 0;

unsigned long previousServoMillis = 0;	// the time when the servo was last moved


//================================================================================
void setup() {

  Serial.begin(9600);
  Serial.println("BridgeControl.ino");  // so we know what sketch is running
  
      // set the Led pins as output:
  pinMode(led_A_Pin, OUTPUT);
 
  myservo.write(servoPosition); // sets the initial position
  myservo.attach(servoPin);
 
}

//================================================================================
void loop() {

      // Notice that none of the action happens in loop() apart from reading millis()
      //   it just calls the functions that have the action code

  currentMillis = millis();   // capture the latest value of millis()
                              //   this is equivalent to noting the time from a clock
                              //   use the same time for all LED flashes to keep them synchronized
  

  updateLed_A_State();
  switchLeds();
  servoSweep();
}

//========================================

void updateLed_A_State()
{
  switch( led_A_State )
  {
    case LOW:
      if (currentMillis - previousLed_A_Millis >= led_A_Interval)
      {
        led_A_State = HIGH;
        previousLed_A_Millis = currentMillis;
      }
    
    case HIGH:
      if (currentMillis - previousLed_A_Millis >= blinkDuration) 
      {
        led_A_State = LOW;
        previousLed_A_Millis = currentMillis;
      }
    default:
      ;
  }
}

//========================================

void switchLeds() {
      // this is the code that actually switches the LEDs on and off

  digitalWrite(led_A_Pin, led_A_State);
}

//======================================

void servoSweep() {

      // this is similar to the servo sweep example except that it uses millis() rather than delay()

      // nothing happens unless the interval has expired
      // the value of currentMillis was set in loop()
  
  if (currentMillis - previousServoMillis >= servoInterval) {
        // its time for another move
    previousServoMillis += servoInterval;
    
    servoPosition = servoPosition + servoDegrees; // servoDegrees might be negative

    if (servoPosition <= servoMinDegrees) {
          // when the servo gets to its minimum position change the interval to change the speed
       if (servoInterval == servoSlowInterval) {
         servoInterval = servoFastInterval;
       }
       else {
        servoInterval = servoSlowInterval;
       }
    }

    if ((servoPosition >= servoMaxDegrees) || (servoPosition <= servoMinDegrees))  {
          // if the servo is at either extreme change the sign of the degrees to make it move the other way
      servoDegrees = - servoDegrees; // reverse direction
          // and update the position to ensure it is within range
      servoPosition = servoPosition + servoDegrees; 
    }
    
        // make the servo move to the next position
    myservo.write(servoPosition);
        // and record the time when the move happened

  }
  
}

//========================================END