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

Servo diverter;  //Making an instance for my servo used to divert the marbles to one path or the other

//defining inputs

const int lightMode = 4;
const int lightToggle = 6;

const int pathMode = 2;
const int pathToggle = 3;


//defining outputs

const int servoPosition = 5;

const int servoPowerRelay = 7;
const int motorPowerRelay = 8;

const int red = 10;
const int yellow = 11;
const int blue = 12;
const int green = 13;




// defining variables and constants

bool pathAuto = false;   //this will be reference to tell if pathing is in automatic or manual
bool lightAuto = false;  //this will be reference to tell if lighting is in automatic or manual

const int pathRate = 777;  // 4000 life too short //interval for switching paths when in automatic
const int lightRate = 166;  // 500 life too short //interval for switching LEDs when in automatic

bool pathA = true;  //one servo value found by trial and error to effectively path the marble
//const int pathB = false;  //the other servo value found by trial and error to effectively path the marble
//int path = pathA;      //holder for current path state

const int buttonPress = 300;  //this will be referenced as part of a software debounce

unsigned long currentMillis = 0;           //this will be used as an ever progressing timeline
unsigned long lightModeButtonMillis = 0;   //this will be referenced as part of a software debounce
unsigned long lightStateButtonMillis = 0;  //this will be referenced as part of a software debounce
unsigned long servoModeButtonMillis = 0;   //this will be referenced as part of a software debounce
unsigned long servoStateButtonMillis = 0;  //this will be referenced as part of a software debounce
unsigned long pathSwitched = 0;            // this will hold the time that the path last changed; used in automatic mode
unsigned long lightSwitched = 0;           // this will hold the time that the LED tower last changed; used in automatic mode


void setup() {


  Serial.begin(9600);

  pinMode(lightMode, INPUT_PULLUP);    //this will look for UI to switch between manual and automatic lighting
  pinMode(lightToggle, INPUT_PULLUP);  //this will look for UI to switch current light state when in manual

  pinMode(pathMode, INPUT_PULLUP);    //this will look for UI to switch between manual and automatic pathing
  pinMode(pathToggle, INPUT_PULLUP);  //this will look for UI to switch current path state when in manual

  diverter.attach(5);  //this will send one of two values to the servo for pathing control

  //these will set two pins to output and constant high as they're energizing the optoisolator on power relays
  pinMode(servoPowerRelay, OUTPUT);
  digitalWrite(servoPowerRelay, HIGH);
  pinMode(motorPowerRelay, OUTPUT);
  digitalWrite(motorPowerRelay, HIGH);

  //these will be the wires activating a battery of relays feeding the LED arrays
  pinMode(red, OUTPUT);
  pinMode(yellow, OUTPUT);
  pinMode(blue, OUTPUT);
  pinMode(green, OUTPUT);

  //this sets up the two UI controlled systems in a default starting state so that an LED is lit and the servo is positioned properly in case the system was previously shut off
  //while the servo was mid move
  //delay(2000);
  diverter.write(0);
  digitalWrite(random(10, 14), HIGH);  //this selects a random LED output pin to activate
  //digitalWrite(blue, HIGH);
  //Serial.println("end of setup");
  //delay(5000);
}

void loop() {

  //this will take the current reading of millis and pass it as a placeholder and then run through the functions which are where the logic lies.

  //Serial.println("loop");

  currentMillis = millis();

  //Serial.println("currentMillis");

  switchLightMode();

  //Serial.println("lightMode");

  switchPathMode();

  //Serial.println("pathMode");

  changeLED();

  //Serial.println("ledChange");

  changePath();

  //Serial.println("pathChange");
}

void switchLightMode() {

  //this function debounces by referencing variables and when the button is pressed, changes the state of the lightAuto bool

  if (millis() - lightModeButtonMillis >= buttonPress) {
    if (digitalRead(lightMode) == LOW) {
      lightAuto = !lightAuto;
      lightModeButtonMillis = millis();
      Serial.print("light Mode  ");
      Serial.println(lightAuto ? "automatic" : "manual");     
      //delay(500);
    }
  }
}

void switchPathMode() {

  //this function debounces by referencing variables and when the button is pressed, changes the state of the lightAuto bool

  if (millis() - servoModeButtonMillis >= buttonPress) {
    if (digitalRead(pathMode) == LOW) {
      pathAuto = !pathAuto;
      servoModeButtonMillis = millis();
      Serial.print("pathMode ");
      Serial.println(pathAuto ? "automatic" : "manual");
      //delay(500);
    }
  }
}

void changeLED() {
  int theLED = 0;

  if (lightAuto == true) {
    //Serial.println("lightAuto == true");
    if (currentMillis - lightSwitched >= lightRate) {
      digitalWrite(red, LOW);
      digitalWrite(yellow, LOW);
      digitalWrite(blue, LOW);
      digitalWrite(green, LOW);
      digitalWrite(random(10, 14), HIGH);  //this selects a random LED output pin to activate
      lightSwitched = currentMillis;
    }
  } else {
    if (millis() - lightStateButtonMillis >= buttonPress) {
      if (digitalRead(lightToggle) == LOW) {
        digitalWrite(red, LOW);
        digitalWrite(yellow, LOW);
        digitalWrite(blue, LOW);
        digitalWrite(green, LOW);
        theLED = random(10, 14);
        digitalWrite(theLED, HIGH);  //this selects a random LED output pin to activate
        lightStateButtonMillis = millis();
        Serial.print("lightSwitch to ");
        Serial.println(theLED);
        //delay(500);
      }
    }
  }
}

void changePath() {
  if (pathAuto == true) {
//    Serial.println("pathMode auto");
    //delay(500);
    if (currentMillis - pathSwitched >= pathRate) {
      if (pathA == true) {
        diverter.write(0);
        pathA = false;
        pathSwitched = currentMillis;
        Serial.println("position 0");
      } else {
        diverter.write(20);
        pathA = true;
        pathSwitched = currentMillis;
        Serial.println("position 20");
      }
    }
  } else {
//    Serial.println("pathMode manual");
    //delay(500);
//    Serial.print("pathToggleValue:  ");
//    Serial.println(digitalRead(pathToggle));
    if (currentMillis - servoStateButtonMillis >= buttonPress) {
      if (digitalRead(pathToggle) == LOW) {
        if (pathA != true) {
          diverter.write(0);
          Serial.println("pathA is not true");
          pathA = true;
          servoStateButtonMillis = currentMillis;
          //delay(500);
        }
        else {
          diverter.write(20);
          Serial.println("pathA is true");
          pathA = false;
          servoStateButtonMillis = currentMillis;
          //delay(500);
        }
      }
    }
  }
}






        /*
        //if (millis() - servoStateButtonMillis >= buttonPress && digitalRead(pathToggle == LOW)){
        Serial.println("pathSwitchButton");
        delay(500);
        Serial.println(pathA);
        if (pathA != true) {
          Serial.println("if block");
          delay(500);
          diverter.write(0);
          pathA = true;
          servoStateButtonMillis = currentMillis;
          Serial.println(pathA);
        } else {
          Serial.println("else block");
          delay(500);
          diverter.write(20);
          pathA = false;
          servoStateButtonMillis = currentMillis;
          Serial.println(pathA);
        }
      }
    }
  }
} */