//For Debugging Purposes
int lastSignal[99] = {0};
unsigned long lastDebounceTime_print[4] = {0, 0, 0, 0};

//For Output Pins and Variables
const int outPin[4] = {10, 9, 8, 7};
int mdl_W   = outPin[0];
int mdl_Y   = outPin[1];
int stock_H = outPin[2];
int horn    = outPin[3];
bool outPinState[4] = {LOW};
unsigned long previousMillis_out[4] = {0};

//For Button attached
const int btnPin[4] = {2, 3, 4, 5};
int Mode1 = btnPin[0];
int Mode2 = btnPin[1];
int HighSW = btnPin[2];
int HornSW = btnPin[3];
int debounceDelay = 100;  // the debounce time, in milliseconds

//the number of array here corresponds to the number of pins attached
unsigned long lastDebounceTime_btn[4] = {0, 0, 0, 0};  // the last time each button was pressed
int lastButtonState[4] = {HIGH};    // the previous state of each button
int buttonState[4] = {0};    // the previous state of each button

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);

  //Outputs
  pinMode(mdl_W, OUTPUT);
    digitalWrite(mdl_W, LOW);
  pinMode(mdl_Y, OUTPUT);
    digitalWrite(mdl_Y, LOW);
  pinMode(stock_H, OUTPUT);
    digitalWrite(stock_H, LOW);
  pinMode(horn, OUTPUT);
    digitalWrite(horn, LOW);  

  //Inputs
  pinMode(A0, INPUT);
  pinMode(btnPin[0], INPUT_PULLUP);
  pinMode(btnPin[1], INPUT_PULLUP);
  pinMode(btnPin[2], INPUT_PULLUP);
  pinMode(btnPin[3], INPUT_PULLUP);
  
}

//function for blinkingLED
void blinkOUT(int id, int LEDPin, int delayTime, bool &LEDState) {
  //static unsigned long previousMillis = 0;
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis_out[id] >= delayTime) {
    previousMillis_out[id] = currentMillis;
    LEDState = !LEDState;
    digitalWrite(LEDPin, LEDState);
  }
}

//function for blinkingLED
void blinkOUTRev(int id, int LEDPin, int delayTime, bool &LEDState) {
  //static unsigned long previousMillis = 0;
  unsigned long currentMillis = millis();
  if (currentMillis - previousMillis_out[id] >= delayTime) {
    previousMillis_out[id] = currentMillis;
    LEDState = !LEDState;
    digitalWrite(LEDPin, !LEDState);
  }
}

//function for converting potVal to desired value
int analogMapped (int potPin, int minVal, int maxVal) {
  //reads the raw value of the analog attached
  int potRaw = analogRead(potPin);

  //Maps and returns the mapped values of the potRaw
  unsigned long potVal = map(potRaw, 0, 1024, minVal, maxVal);
  return potVal;
}

//function for reading buttons with debounce
bool btnRead(int buttonIndex, int pin) {
  int buttonState = digitalRead(pin);

  if (buttonState != lastButtonState[buttonIndex]) {
    lastDebounceTime_btn[buttonIndex] = millis();
  }

  if ((millis() - lastDebounceTime_btn[buttonIndex]) > debounceDelay) {
    if (buttonState != buttonState) {
      buttonState = digitalRead(pin);
    }
  }

  lastButtonState[buttonIndex] = buttonState;
  return buttonState;
}

//function for debugging purposes
void printOnce(String msg_HIGH, String msg_LOW, int id, int signal, int *lastSignal, int debounceTime) {
  //lastDebounceTime_print[id];
  unsigned long currentTime = millis();

  if (signal != lastSignal[id] && (currentTime - lastDebounceTime_print[id]) > debounceTime) {
    //Serial.println("Signal " + String(id) + " received: " + String(signal));
    if(signal == LOW) {
      Serial.println(msg_HIGH);
    } else {
      Serial.println(msg_LOW);
    }
    lastSignal[id] = signal;
    lastDebounceTime_print[id] = currentTime;
  }
}



void loop() {
  // put your main code here, to run repeatedly:
  //blinkOUT(ledPin1, analogMapped(A0, 50, 500), ledPin1State);
  //Serial.println(analogMapped(A0, 50, 500));
  serialInfo();
  output();
  
}

void serialInfo() {

  int sig_Mode1 = btnRead(0, Mode1);
  int sig_Mode2 = btnRead(1, Mode2);
  int sig_High = btnRead(2, HighSW);
  int sig_Horn = btnRead(3, HornSW);
  int serialDebounce = 100;
  
  //printOnce(Msg when HIGH, Msg when LOW, id for:, lastSignal, lastDebounceTime_print, serialDebounce);
  printOnce("Mode 1 Activated.",  "Mode 1 Deactivated.",  0,  sig_Mode1,  lastSignal, 100);
  printOnce("Mode 2 Activated.",  "Mode 2 Deactivated.",  1,  sig_Mode2,  lastSignal, 100);
  printOnce("High SW Activated.", "High SW Deactivated.", 2,  sig_High,   lastSignal, 100);
  printOnce("Horn SW Activated.", "Horn SW Deactivated.", 3,  sig_Horn,   lastSignal, 100);
  
}

void output() {
  if (btnRead(0, Mode1) == LOW && btnRead(1, Mode2) == HIGH) {
    if (btnRead(3, HornSW) == LOW && btnRead(2, HighSW) == LOW)
    {
      blinkOUT(0, mdl_W,   analogMapped(A0, 50, 500), outPinState[0]);
      blinkOUTRev(1, mdl_Y,   analogMapped(A0, 50, 500), outPinState[1]);
      blinkOUT(2, stock_H, analogMapped(A0, 50, 500), outPinState[2]);
      blinkOUT(3, horn, analogMapped(A0, 50, 500), outPinState[3]);
    } 
    else if (btnRead(3, HornSW) == LOW && btnRead(2, HighSW) == HIGH) 
    {
      blinkOUTRev(0, mdl_W,   analogMapped(A0, 50, 500), outPinState[0]);
      blinkOUT(1, mdl_Y,   analogMapped(A0, 50, 500), outPinState[1]);
      blinkOUT(2, stock_H, analogMapped(A0, 50, 500), outPinState[2]);
      blinkOUT(3, horn, analogMapped(A0, 50, 500), outPinState[3]);
    } 
    else if (btnRead(2, HighSW) == LOW && btnRead(3, HornSW) == HIGH) 
    {
      digitalWrite(mdl_W, LOW);
      digitalWrite(mdl_Y, HIGH);
      digitalWrite(stock_H, HIGH);
      digitalWrite(horn, LOW);
      outPinState[0] = outPinState[1] = outPinState[2] = outPinState[3];
    }
    else 
    {
      digitalWrite(mdl_W, HIGH);
      digitalWrite(mdl_Y, LOW);
      digitalWrite(stock_H, LOW);
      digitalWrite(horn, LOW);
      outPinState[0] = outPinState[1] = outPinState[2] = outPinState[3];
    }
  }

  else if (btnRead(0, Mode1) == HIGH && btnRead(1, Mode2) == LOW) {
    if (btnRead(3, HornSW) == LOW && btnRead(2, HighSW) == LOW)
    {
      blinkOUT(0, mdl_W,   analogMapped(A0, 50, 500), outPinState[0]);
      blinkOUT(2, stock_H, analogMapped(A0, 50, 500), outPinState[2]);
      blinkOUT(3, horn, analogMapped(A0, 50, 500), outPinState[3]);
    } 
    else if (btnRead(3, HornSW) == LOW && btnRead(2, HighSW) == HIGH) 
    {
      blinkOUT(0, mdl_W,   analogMapped(A0, 50, 500), outPinState[0]);
      blinkOUT(2, stock_H, analogMapped(A0, 50, 500), outPinState[2]);
      blinkOUT(3, horn, analogMapped(A0, 50, 500), outPinState[3]);
    } 
    else if (btnRead(2, HighSW) == LOW && btnRead(3, HornSW) == HIGH) 
    {
      digitalWrite(mdl_W, HIGH);
      digitalWrite(stock_H, HIGH);
      digitalWrite(horn, LOW);
      outPinState[0] = outPinState[1] = outPinState[2] = outPinState[3];
    }
    else 
    {
      digitalWrite(mdl_W, LOW);
      digitalWrite(mdl_Y, LOW);
      digitalWrite(stock_H, LOW);
      digitalWrite(horn, LOW);
      outPinState[0] = outPinState[1] = outPinState[2] = outPinState[3];
    }
  }

  else if (btnRead(2, HighSW) == LOW && btnRead(0, Mode1) == HIGH && btnRead(1, Mode2) == HIGH)
  {
    if (btnRead(3, HornSW) == LOW) 
    {
      blinkOUT(2, stock_H, analogMapped(A0, 50, 500), outPinState[2]);
      blinkOUT(3, horn, analogMapped(A0, 50, 500), outPinState[3]);
    }
    else 
    {
      digitalWrite(stock_H, HIGH);
      digitalWrite(horn, LOW);
      digitalWrite(mdl_W, LOW);
      digitalWrite(mdl_Y, LOW);
      outPinState[0] = outPinState[1] = outPinState[2] = outPinState[3];
    }
    
  }
  else if (btnRead(3, HornSW) == LOW && btnRead(0, Mode1) == HIGH && btnRead(1, Mode2) == HIGH)
  {
    blinkOUT(2, stock_H, analogMapped(A0, 50, 500), outPinState[2]);
    blinkOUT(3, horn, analogMapped(A0, 50, 500), outPinState[3]);
  }

  else
  {
    digitalWrite(mdl_W, LOW);
    digitalWrite(mdl_Y, LOW);
    digitalWrite(stock_H, LOW);
    digitalWrite(horn, LOW);
    outPinState[0] = outPinState[1] = outPinState[2] = outPinState[3];
  }
  
}