#include <Servo.h>

Servo myservo;
int servoPin = 9;

int STOPpin = 4;
int CWpin = 3;
int CCWpin = 5;
int LED1pin = 6;  // LED for CW
int LED2pin = 7;  // LED for STOP
int LED3pin = 8;  // LED for CCW

int sc[] = {180, 110, 0};
String scText[] = {"CCW", "STOP", "CW"};
int statusText;
int CWBS, CCWBS, SBS;

void setup() {
  Serial.begin(9600);
  pinMode(STOPpin, INPUT_PULLUP);
  pinMode(CCWpin, INPUT_PULLUP);
  pinMode(CWpin, INPUT_PULLUP);

  pinMode(LED1pin, OUTPUT);
  pinMode(LED2pin, OUTPUT);
  pinMode(LED3pin, OUTPUT);

  myservo.attach(servoPin);
  myservo.write(sc[1]);
  statusText = 1;
}

void loop() {
  CCWBS = digitalRead(CCWpin);
  SBS = digitalRead(STOPpin);
  CWBS = digitalRead(CWpin);

  if (CCWBS == LOW) {
    servoCommand(0);
    digitalWrite(LED3pin, HIGH);
    digitalWrite(LED2pin, LOW);
    digitalWrite(LED1pin, LOW);
    for (int pos = 180; pos >= 0; pos -= 1) {
      myservo.write(pos);
      delay(15);
    }
  }
  else if (SBS == LOW) {
    servoCommand(1);
    digitalWrite(LED2pin, HIGH);
    digitalWrite(LED1pin, LOW);
    digitalWrite(LED3pin, LOW);
  }
  else if (CWBS == LOW) {
    servoCommand(2);
    digitalWrite(LED1pin, HIGH);
    digitalWrite(LED2pin, LOW);
    digitalWrite(LED3pin, LOW);
    for (int pos = 0; pos <= 180; pos += 1) {
      myservo.write(pos);
      delay(15);
    }
  }
  Serial.println(scText[statusText]);
  delay(50);
}

void servoCommand(int n) {
  statusText = n;
  myservo.write(sc[n]);
  Serial.print("Going to ...");
  Serial.print(scText[n]);
  Serial.print(" (");
  Serial.print(sc[n]);
  Serial.println(")");
}