// include the servo library
#include <Servo.h>

// create servo object to control a servo
Servo Servo_1;
Servo Servo_2;

// constants: input and output pins
const int SWITCH_PIN = 4;
const int SERVO_1_PIN = 9;
const int SERVO_2_PIN = 10;

// constants: the trap can be open or close.
// easier to understand than HIGH or LOW
const int CLOSED = LOW;
const int OPEN = HIGH;

// constants: positions of servo end points and positioning delay in microseconds
const int SERVO_1_CLOSED = 7;
const int SERVO_1_OPEN = 100;
const int SERVO_2_CLOSED = 159;
const int SERVO_2_OPEN = 64;
const int SERVO_DELAY = 10;

// variables
int switch_state = LOW;     // the current state of the input pin
int trap_state = OPEN;      // will a high switch state change the servo position

void setup() {
  // setup code here, run once after reset or power on:

  // setting pins to input, and output
  pinMode(SWITCH_PIN, INPUT);
  pinMode(LED_BUILTIN, OUTPUT);

  // initialize serial connection
  Serial.begin(9600);

  // attach the servos
  Servo_1.attach(SERVO_1_PIN);
  Servo_2.attach(SERVO_2_PIN);

  // moving servos to defined position
  Servo_1.write(SERVO_1_OPEN);
  Servo_2.write(SERVO_2_OPEN);

  // read the current switch state
  switch_state = digitalRead(SWITCH_PIN);
}


void loop() {
  // main code, run repeatedly:

  // MANUAL SWITCH STATE
  // -------------------

  // read the current state of the input
  int current_switch_state = digitalRead(SWITCH_PIN);

  // change trap position if the switch state has changed
  if (current_switch_state != switch_state) {

    // register the change
    switch_state = current_switch_state;

    if (switch_state == HIGH) {
      // show that there is currently a input signal
      // and close the trap
      digitalWrite(LED_BUILTIN, HIGH);
      trap_state = close_trap();
    } else {
      // show that there is currently no input signal
      // and open the trap
      digitalWrite(LED_BUILTIN, LOW);
      trap_state = open_trap();
    }
  }


  // COMMANDS SEND OVER USB
  // ----------------------

  // read the incoming data
  char inByte = Serial.read(); 
  
  if (inByte == 'o') {
    // the open trap command was sent
    trap_state = open_trap();
  } else if (inByte == 'c') {
    // the close trap command was sent
    trap_state = close_trap();
  }
}

int open_trap() {
  Servo_1.write(SERVO_1_OPEN);
  Servo_2.write(SERVO_2_OPEN);
  return OPEN;
}

int close_trap() {
  Servo_1.write(SERVO_1_CLOSED);
  Servo_2.write(SERVO_2_CLOSED);
  return CLOSED;
}