#include <arduino-timer.h>

#define DOORRUN 2
#define DOOREND 4
#define TRIGGER 7

#define LED0 3
#define LED1 5
#define LED2 6
#define LED3 9
#define LED4 10
#define LED5 11

void setup(){
  pinMode(DOOREND, INPUT_PULLUP);
  pinMode(TRIGGER, INPUT_PULLUP);
  Serial.begin(115200);
}

//Led and initial speed settings
int maxDelay = 400;
int maxBright = 100;
int ledCount = 6;
byte ledPin[] = {LED0, LED1, LED2, LED3, LED4, LED5};
int blinkTarget = 5; //Twice the amount of times you want the light to blink

//More important settings, don't change unless you know what you're doing
int tickDelay = 10;
int analogRes = 255;

//No need to touch these
int changeDelay = maxDelay;
int angleCount;
int currentLED;
int brightness;
int blinkCounter = 0;
bool blinkOn = true;
Timer<1> changeLED;
enum {waiting, switchMade, doorActivate, lightShowBlink, lightShowChase, lightShowPulse, lightShowBreak, doorOpen, doorReturn, switchUnmade, gameReset};
unsigned char state = waiting;

void allLedDW(bool ledOn){
  int i = 0;
  while(i<ledCount){
    digitalWrite(ledPin[i], ledOn);
    i++;
  }
}

void allLedAW(int level){
  int i = 0;
  while(i<ledCount){
    analogWrite(ledPin[i], level);
    i++;
  }
}

int sineCounter(int maxValue){
  float angle = radians(angleCount);
  int changeTarget = (maxValue/2) + ((maxValue/2) * sin(angle));
  if (angleCount < 360){
    angleCount++;
  } else{
    angleCount = 0;
  }
  return changeTarget;
}

void fullBlink(){
  allLedDW(blinkOn);
  blinkOn = !blinkOn;
  blinkCounter++;
}

void lightChase(){
  allLedAW(0);
  analogWrite(ledPin[currentLED], analogRes);
  analogWrite(ledPin[currentLED - 1], analogRes);
  if(currentLED == 0){
    digitalWrite(ledPin[ledCount - 1], analogRes);
    currentLED++;
    changeDelay = sineCounter(maxDelay);
    return;
  } else if(currentLED < ledCount - 1){
    currentLED++;
    changeDelay = sineCounter(maxDelay);
    return;
  } else {
    currentLED = 0;
    changeDelay = sineCounter(maxDelay);
  }
}

void lightDim(){
  int trueBright = (brightness * analogRes)/100;
  allLedAW(trueBright);
}

void lightPulse(){
  brightness = sineCounter(maxBright);
  lightDim();
}

void doorOpened(){
  allLedDW(1);
  state = doorOpen;
}

void stateMachine(){
  switch (state) {

    case waiting:
      if(!digitalRead(TRIGGER)){
        state=switchMade;
      }
      break;

    case switchMade:
      state = doorActivate;
      break;

    case doorActivate:
      digitalWrite(DOORRUN, 1);
      state = lightShowBlink;
      break;

    case lightShowBlink:
      if(blinkCounter > blinkTarget){
        angleCount = 180;
        currentLED = 0;
        state = lightShowChase;
      } else if(changeLED.empty()) {
        changeLED.in(maxDelay, fullBlink);
      }
      break;

    case lightShowChase:
      if(changeLED.empty()){
        changeLED.in(changeDelay, lightChase);
      }
      if(changeDelay < tickDelay){
        state = lightShowPulse;
      }
      break;

    case lightShowPulse:
      lightPulse();
      if(!digitalRead(DOOREND) && changeLED.empty()){
        changeLED.in(5000, doorOpened);
      }
      break;

    case lightShowBreak:
      changeLED.cancel();
      break;

    case doorOpen:
      break;

    case switchUnmade:
      break;

    case doorReturn:
      break;

    case gameReset:
      break;
  }
}

void debugPrint(){
  Serial.print("LED 1: ");
  Serial.print(digitalRead(ledPin[0]));
  Serial.print(" ");
  Serial.print("LED 2: ");
  Serial.print(digitalRead(ledPin[1]));
  Serial.print(" ");
  Serial.print("LED 3: ");
  Serial.print(digitalRead(ledPin[2]));
  Serial.print(" ");
  Serial.print("LED 4: ");
  Serial.print(digitalRead(ledPin[3]));
  Serial.print(" ");
  Serial.print("LED 5: ");
  Serial.print(digitalRead(ledPin[4]));
  Serial.print(" ");
  Serial.print("LED 6: ");
  Serial.print(digitalRead(ledPin[5]));
  Serial.print(" ");
  Serial.print("Blink State: ");
  Serial.print(blinkOn);
  Serial.print(" ");
  Serial.print("state: ");
  Serial.print(state);
  Serial.print(" ");
  Serial.println("uT");
}

void debugPrint2(){
  int i = 0;
  int count = 0;
  while (i < ledCount){
    count = count + digitalRead(ledPin[i]);
    i++;
  }
  Serial.println(count);
}

void loop(){
  stateMachine();
  Serial.println(state);
  //debugPrint();
  //debugPrint2();
  changeLED.tick();
  delay(tickDelay);
}