//#include <Servo.h>
#define solenoidTimeOn 1000
#define solenoidTimeOff 12500
#define SERVO_WAIT_TIME 4000



#include <Adafruit_TiCoServo.h>
#include <Adafruit_NeoPixel.h>
#define LED_PIN A0
#define solenoid A1
Adafruit_NeoPixel strip = Adafruit_NeoPixel(24, LED_PIN, NEO_GRB + NEO_KHZ800);
#define READY 0
#define MOVING 1
#define ANGLE180 2
#define WAITING 4

#define WAIT_TRIGGER 0
#define TRIGGERED 1
#define LOCKED 2

#define DEBOUNCE_DELAY 100

unsigned long currentTime;
unsigned long currentTime2;
unsigned long tNow;
int servoPin = 9;
int safetySw = 2;
int servoSw = 3;
int trigger = 4;
int servoStatus = READY;
int debounce = 0;

bool safetyStatus=1;
bool servoSwStatus;
bool solenoidState = 0;

int solenoidStatus = WAIT_TRIGGER;

//Servo servo1;
Adafruit_TiCoServo servo1; // create servo object to control a servo
void setup() {
  Serial.begin(115200);
  pinMode(safetySw, INPUT_PULLUP);
  pinMode(servoSw, INPUT_PULLUP);
  pinMode(trigger, INPUT_PULLUP);
  pinMode(solenoid,OUTPUT);
  digitalWrite(solenoid,LOW);
  strip.begin();
  strip.setBrightness(100);
  strip.show(); // Initialize all pixels to 'off'
  // put your setup code here, to run once:
  servo1.attach(servoPin);
  colorWipe(strip.Color(0, 0, 255), 1); // Red
  servo1.write(0);
  delay(100);

  currentTime = millis();
}

void loop() {
  // put your main code here, to run repeatedly:
  handleSolenoid();
  handleServo();
  handleLedRing();
  if((millis()-tNow) >= 1000){
    tNow = millis();
    printAll();
  }
}

// Fill the dots one after the other with a color
void colorWipe(uint32_t c, uint8_t wait) {
  for (uint16_t i = 0; i < strip.numPixels(); i++) {
    strip.setPixelColor(i, c);
    strip.show();
    delay(wait);
  }
}

void handleSolenoid(){
  bool triggerState = digitalRead(trigger);
  switch(solenoidStatus){
    case WAIT_TRIGGER:
      if((triggerState == 0) &&(safetyStatus == 0)){
        solenoidState = 1;
        currentTime2 = millis();
        solenoidStatus = TRIGGERED;
        Serial.println("Solenoid ON");
        if(safetyStatus == 0){
          colorWipe(strip.Color(255, 0, 0), 1); // red
        }       
      }
      break;
    case TRIGGERED:
      if((millis()-currentTime2) >= solenoidTimeOn){
        solenoidStatus = LOCKED;
        solenoidState = 0;
        currentTime2 = millis();
        Serial.println("Solenoid Locked");
      }
      break;
    case LOCKED:
      if((millis()-currentTime2) >= solenoidTimeOff){
        solenoidStatus = WAIT_TRIGGER;
        if(safetyStatus == 0){
          colorWipe(strip.Color(0, 255, 0), 1); // green
        }  
      }
      break;
    default:
      break;
  }
  digitalWrite(solenoid,solenoidState);
}

void handleServo(){
  servoSwStatus = digitalRead(servoSw);
  switch (servoStatus) {
    case READY:
      if (servoSwStatus == 1) {
        debounce++;
        delay(1);
        if(debounce >= DEBOUNCE_DELAY){
          servo1.write(180);
          servoStatus = MOVING;
          Serial.println("Servo Move");
          debounce = 0;
        }        
      }
      break;
    case MOVING:
      currentTime = millis();
      servoStatus = ANGLE180;
      Serial.println("Servo at Angle 180");
      break;
    case ANGLE180:
      if ((millis() - currentTime) >= SERVO_WAIT_TIME) {
        servo1.write(0);
        servoStatus = WAITING;
        Serial.println("Servo wait");
      }
      break;
    case WAITING:
      if (servoSwStatus == 0) {
        servoStatus = READY;
        Serial.println("Servo READY");
        delay(100);
      }
      break;
    default:
      break;
  }
}

void handleLedRing(){
  if (digitalRead(safetySw) == 1) {
    if (safetyStatus == 1) {
      safetyStatus = 0;
      if(solenoidStatus == WAIT_TRIGGER){
        colorWipe(strip.Color(0, 255, 0), 1); // green
      }
      else{
        colorWipe(strip.Color(255, 0, 0), 1); // green
      }
      
    }  
  }
  else {
    if (safetyStatus == 0) {
      colorWipe(strip.Color(00, 0, 255), 1); // blue
      safetyStatus = 1;
    }
  }

}

void printAll(){
  if(solenoidStatus != WAIT_TRIGGER){
    Serial.println((millis()-currentTime2)/1000 +1);
  }
}