#include <Servo.h>

// create servos
Servo pull_servo;
Servo moon_servo;

// analog input
int door_sensor_pin   = 1;
int top_sensor_pin    = 2;
int bottom_sensor_pin = 3;
int pot1_pin          = 4;
int pot2_pin          = 5;

// digital input
int button1_pin       = 7;
int button2_pin       = 8;

// digital output
int up_motor_pin      = 4;  // needs to be left motor
int down_motor_pin    = 3; // needs to be right motor
int lightning_pin     = 6;

// initial values
int start_door_val   = 0;
int start_top_val    = 0;
int start_bottom_val = 0;
int start_pot1_val   = 0;
int start_pot2_val   = 0;

// loop analog values
int door_val      = 0;
int top_val       = 0;
int bottom_val    = 0;
int pot1_val      = 0;
int pot2_val      = 0;

// loop digital values
int button1 = 0;
int button2 = 0;

// general variables
int top_bottom_bias = 0;
int top_bottom_trigger = 0;
int door_bias = 0;
int door_trigger = 0;
int curr_pull_servo_pos = 0;
int prev_pull_servo_pos = 0;
int countdown = 0;

// defaults
int start_pull_servo_pos = 0;
int end_pull_servo_pos = 160;
int start_moon_servo_pos = 120;
int end_moon_servo_pos = 0;

// States
// -1 = failure to initialize
//  0 = lift plate at bottom, trap armed
//  1 = trap armed, door sensor drop
//  2 = trap armed, door sensor back up
//  4 = running up motor
//  5 = stopped at top
//  9 = running down motor
// 10 = init at top
int state = -1;

void setup () {
  // Serial.begin(9600);  // uncomment this out if you want to use print statements with Serial Monitor 
  // init servos
  pull_servo.attach(11);
  moon_servo.attach(10);
  
  // init digital pins
  pinMode(button1_pin, INPUT);
  pinMode(button2_pin, INPUT);
  pinMode(up_motor_pin, OUTPUT);
  pinMode(down_motor_pin, OUTPUT);
  pinMode(lightning_pin, OUTPUT);
  
  // read inital values
  start_door_val   = analogRead(door_sensor_pin);;
  start_top_val    = analogRead(top_sensor_pin);
  start_bottom_val = analogRead(bottom_sensor_pin);
  start_pot1_val   = analogRead(pot1_pin);
  start_pot2_val   = analogRead(pot2_pin);
  
  // try and figure out where lift plate is.  It needs to start at either the top of the bottom
  // so one of the sensors should be a lot bigger than the other
  int diff = abs(start_top_val - start_bottom_val);
  if (diff > 40) {
    // determin if it's top or bottom
    if (start_top_val > start_bottom_val) {
      state = 10;
      // lift up moon just incase it needs to be installed
      moon_servo.write(end_moon_servo_pos);
      top_bottom_trigger = start_top_val;
    }
    else {
      state = 0;  
      moon_servo.write(start_moon_servo_pos);
      top_bottom_trigger = start_bottom_val;
    }
    pull_servo.write(start_pull_servo_pos);
    curr_pull_servo_pos = start_pull_servo_pos;
    
    // set door value trigger to be if it drops by half of inital value
    // photocell increases with less light
    door_trigger = start_door_val+250;
  }
  else {
    // values too close, error
    state = -1;
  }
}

void loop () {
  if (state != -1) {
    // read analog values
    door_val   = analogRead(door_sensor_pin);;
    top_val    = analogRead(top_sensor_pin);
    bottom_val = analogRead(bottom_sensor_pin);
    pot1_val   = analogRead(pot1_pin);
    pot2_val   = analogRead(pot2_pin);      
    
    // reading digital values
    button1 = digitalRead(button1_pin);
    button2 = digitalRead(button2_pin);

    // do general work
    // set top/bottom bias
    top_bottom_bias = map(pot1_val,0,1024,-100,100);


    switch(state) {
      case 0: //  0 = lift plate at bottom, trap armed
      // in this state, pot 2 trims pull servo
      prev_pull_servo_pos = curr_pull_servo_pos;
      curr_pull_servo_pos = map(pot2_val,0,1024,0,160);  // these cheap servos only go to 160
      if (curr_pull_servo_pos != prev_pull_servo_pos) {
        pull_servo.write(curr_pull_servo_pos);
      }
      
      if ((door_val - door_bias) >= door_trigger) {
        state = 1;
      }
      if (button1 == LOW) {
        delay(750); // to ensure no button skipping
        state = 4;
      }
      break;

      case 1: //  1 = trap armed, door sensor drop
      // wait till door goes back up
      // (comes within 25 of original value)
      if ((door_val - door_bias) <= start_door_val) {
        state = 2;
      }
      break;

      case 2: //  2 = trap armed, door sensor back up
        pull_servo.write(end_pull_servo_pos);
        moon_servo.write(end_moon_servo_pos);
        state = 4;
      break;
      
      case 4: //  4 = running up motor
      // looking at top sensor
      if ((top_val - top_bottom_bias) <= top_bottom_trigger) {
        digitalWrite(up_motor_pin,LOW);
        state = 5;
      }
      else {
        digitalWrite(up_motor_pin,HIGH);
      }
      break;

      case 5: //  5 = stopped at top
      if (button2 == LOW) {
        delay(750); // to ensure no button skipping
        // lower moon down
        moon_servo.write(start_moon_servo_pos);
        pull_servo.write(start_pull_servo_pos);
        state = 9;
      }
      
      // send out the white lightning
      digitalWrite(lightning_pin,HIGH);
      delay(100);
      digitalWrite(lightning_pin,LOW);
      delay(100);
      
      break;

      case 9: //  9 = running down motor
      // looking at bottom sensor
      if ((bottom_val - top_bottom_bias) <= top_bottom_trigger) {
        digitalWrite(down_motor_pin,LOW);
        state = 0;
      }
      else {
        digitalWrite(down_motor_pin,HIGH);
      }
      break;
      
      case 10: // init at top
      // in this state pot 2 sets the door bias
      door_bias = map(pot2_val,0,1024,-100,100);
      // and white lightning light comes on when door is triggered
      if ((door_val - door_bias) <= door_trigger) {
        digitalWrite(lightning_pin,HIGH);
      }
      else {
        digitalWrite(lightning_pin,LOW);
      }
      
      if (button2 == LOW) {
        delay(750); // to ensure no button skipping
        // lower moon down
        moon_servo.write(start_moon_servo_pos);
        pull_servo.write(start_pull_servo_pos);
        state = 9;
      }
      break;
      
      default:
      break;
    }
  }
  else {
    // not initialized properly, blink lighting light
    digitalWrite(lightning_pin,HIGH);
    delay(250);
    digitalWrite(lightning_pin,LOW);
    delay(250);
  }
}