// Used to debounce GPIO Digital Input - https://github.com/thomasfredericks/Bounce2
#include "Bounce2.h"
// Used for Pushbutton handling - https://github.com/poelstra/arduino-multi-button
#include "PinButton.h"

// output assignments
const int ledStatus8 = 8;          // Status LED
const int ledFBremse9 = 9;         // Status LED Feststellbremse
const int RelStandlicht10 = 10;    // Relais 1 - Standlicht
const int RelAbblendlicht11 = 11;  // Relais 2 - Abblendlicht
const int RelFernlicht12 = 12;     // Relais 3 - Fernlicht
const int RelBlinkerLinks13 = 13;  // Relais 4 - Blinker Links
const int RelBlinkerRechts14 = 14; // Relais 5 - Blinker Rechts
const int RelBremslicht15 = 15;    // Relais 6 - Bremslicht
const int RelHorn16 = 16;          // Relais 7 - Horn
const int Out1Motor18 = 18;        // H-Bridge output 1
const int Out2Motor19 = 19;        // H-Bridge output 2

// input assignment
const int InAnalog7 = 7;          // Voltage sensing soldered to A7

// Variables for Battery Voltage sensing and calculation
float vout = 0.0;
float vin = 0.0;
float R1 = 100000.0; // resistance of R1 (100K)
float R2 = 10000.0; // resistance of R2 (10K)
int value = 0;
int BatLOWcounter = 0; // Number of Status LED flashes if BAT is LOW
bool BatLOW = false; // used to handle low battery status during runtime
bool BatidleLOW = false; // used to handle low battery status during startup

// Used for smoothing of analog voltage reading on Pin A7 - copied from   https://www.arduino.cc/en/Tutorial/BuiltInExamples/Smoothing
const int numReadings = 10;
float readings[numReadings];      // the readings from the analog input
int readIndex = 0;              // the index of the current reading
float total = 0;                  // the running total
float average = 0;                // the average
// Voltage Reading Interval
const unsigned long vreadingInterval = 1000;
unsigned long vreadingpreviousMillis = 0;


// Used to trigger a startup light sequence once after zuendung is turned on.
bool startup = true;

// Blinker Interval
const unsigned long BlinkerInterval = 750;
unsigned long BlinkerpreviousMillis = 0;

// Fast Blinker Interval
const unsigned long FBlinkerInterval = 250;
unsigned long FBlinkerpreviousMillis = 0;

// Flash Interval
const unsigned long FlashInterval = 100;
unsigned long FlashpreviousMillis = 0;

// Status LED Flashing (used for bat low indication)
unsigned long statusledpreviousMillis = 0;
unsigned long statusledflashpreviousMillis = 0;

// Flash Counter for Startup Blinking
int flashcounter = 0;

// variable for Blinkerstate
int Blinkerstate = 0;
bool RelBlinkerLinks13state = false;
bool RelBlinkerRechts14state = false;

// Blinker Button input using Multi Button library
PinButton buttonBlinker(6);

// Instantiate Bounce2 inputs:
Bounce2::Button switchzuendung = Bounce2::Button(); 
Bounce2::Button switchabblendlicht = Bounce2::Button();
Bounce2::Button switchfernlicht = Bounce2::Button();
Bounce2::Button switchfeststellbremse = Bounce2::Button();
Bounce2::Button sensorfbremsegezogen = Bounce2::Button();
Bounce2::Button sensorfbremsegeloest = Bounce2::Button();
Bounce2::Button sensorbremse = Bounce2::Button();
Bounce2::Button buttonhorn = Bounce2::Button();

#define d0 0 // Zündung Schlüssel Schalter
#define d1 1 // Abblendlicht Schalter
#define d2 2 // Fernlicht Schalter
#define d3 3 // Feststellbremse Schalter
#define d4 4 // Feststellbremse sensor gezogen
#define d17 17 // Feststellbremse sensor gezogen
#define d5 5 // Bremse Sensor
#define d7 7 // Horn Taster

void setup() {
// initialize input:
  switchzuendung.attach (d0, INPUT_PULLUP);
  switchzuendung.interval(5);
  switchzuendung.setPressedState(LOW);
  
  switchabblendlicht.attach (d1, INPUT_PULLUP);
  switchabblendlicht.interval(5);
  switchabblendlicht.setPressedState(LOW);
  
  switchfernlicht.attach (d2, INPUT_PULLUP);
  switchfernlicht.interval(5);
  switchfernlicht.setPressedState(LOW);
  
  switchfeststellbremse.attach (d3, INPUT_PULLUP);
  switchfeststellbremse.interval(5);
  switchfeststellbremse.setPressedState(LOW);
  
  sensorfbremsegezogen.attach (d4, INPUT_PULLUP);
  sensorfbremsegezogen.interval(5);
  sensorfbremsegezogen.setPressedState(LOW);

  sensorfbremsegeloest.attach (d17, INPUT_PULLUP);
  sensorfbremsegeloest.interval(5);
  sensorfbremsegeloest.setPressedState(LOW);
  
  sensorbremse.attach (d5, INPUT_PULLUP);
  sensorbremse.interval(5);
  sensorbremse.setPressedState(LOW);

  buttonhorn.attach (d7, INPUT_PULLUP);
  buttonhorn.interval(5);
  buttonhorn.setPressedState(LOW);

  // set analog input mode for voltage sensing
  pinMode(InAnalog7, INPUT);
  
  // initialize output
  pinMode(ledStatus8, OUTPUT);
  pinMode(ledFBremse9, OUTPUT);
  pinMode(RelStandlicht10, OUTPUT);
  pinMode(RelAbblendlicht11, OUTPUT);
  pinMode(RelFernlicht12, OUTPUT);
  pinMode(RelBlinkerLinks13, OUTPUT);
  pinMode(RelBlinkerRechts14, OUTPUT);
  pinMode(RelBremslicht15, OUTPUT);
  pinMode(RelHorn16, OUTPUT);
  pinMode(Out1Motor18, OUTPUT);
  pinMode(Out2Motor19, OUTPUT);

// Initial Battery Voltage reading and Calculation during setup - copied from https://projecthub.arduino.cc/SetNFix/arduino-voltmeter-3bf7f4#section6
   delay (500);
   value = analogRead(InAnalog7);
   vout = (value * 5.0) / 1024.0;
   vin = vout / (R2/(R1+R2)); 
   if (vin<0.09) {
      vin=0.0;
   }
// initialize all the readings to 0 for Voltage reading on Pin A7:
  for (int thisReading = 0; thisReading < numReadings; thisReading++) {
    readings[thisReading] = 0;
  }

 vin = 12.50; // Use this to set voltage level for testing in WOKWI (only 5V source)
// set the runtime voltage variable = initial reading to bridge the gap until the 10s array is calculated/smoothed.
average = vin;
  // Blink three times when Controller Starts, and Battery is FULL
  if (vin>=12.40){
     while (flashcounter<=2) {
     digitalWrite(RelBlinkerLinks13, HIGH);
     digitalWrite(RelBlinkerRechts14, HIGH);
     delay (500);
     digitalWrite(RelBlinkerLinks13, LOW);
     digitalWrite(RelBlinkerRechts14, LOW);
     delay (300);
     flashcounter++;
     }
  }
  // Blink twice when Controller Starts, and Battery is 50%
  if (vin>=12.00){
     while (flashcounter<=1) {
     digitalWrite(RelBlinkerLinks13, HIGH);
     digitalWrite(RelBlinkerRechts14, HIGH);
     delay (500);
     digitalWrite(RelBlinkerLinks13, LOW);
     digitalWrite(RelBlinkerRechts14, LOW);
     delay (300);
     flashcounter++;
     }
  }
  // Flash Status LED when Controller Starts, and Battery is LOW
  if (vin<12.00){
     BatidleLOW = true;
     while (BatLOWcounter<=20){
     digitalWrite(ledStatus8, HIGH);
     delay (100);
     digitalWrite(ledStatus8, LOW);
     delay (200);
     BatLOWcounter++;
     }
  }
// Activate for debugging
  Serial.begin(9600);
}
// -------------------------------------- LOOP -------------------------------------------------------------
void loop() {
  // continuously read input states
  switchzuendung.update();
  switchabblendlicht.update();
  switchfernlicht.update();
  switchfeststellbremse.update();
  sensorfbremsegezogen.update();
  sensorfbremsegeloest.update();
  sensorbremse.update();
  buttonBlinker.update();
  buttonhorn.update();
  unsigned long currentMillis = millis();
// -------------------------------------- BATTERY MONITOR ----------------------------------------------------
// continuous Battery Voltage reading and Calculation - copied from https://projecthub.arduino.cc/SetNFix/arduino-voltmeter-3bf7f4#section6
   value = analogRead(InAnalog7);
   vout = (value * 5.0) / 1024.0;
   vin = vout / (R2/(R1+R2)); 
   if (vin<0.09) {
      vin=0.0;
   }
   vin = 11.80; // Use this to set voltage level for testing in WOKWI (only 5V source)
// store every 1s the voltage reading from vin. calculate average value after 10 readings.
   if (currentMillis - vreadingpreviousMillis >= vreadingInterval) {
      vreadingpreviousMillis = currentMillis;
      // subtract the last reading:
      total = total - readings[readIndex];
      // read from the sensor:
      readings[readIndex] = vin;
      // add the reading to the total:
      total = total + readings[readIndex];
      // advance to the next position in the array:
      readIndex = readIndex + 1;
   }
  // if we're at the end of the array...
  if (readIndex >= numReadings) {
      average = total / numReadings;
     //   Serial.println(average);
    // ...wrap around to the beginning:
    readIndex = 0;
  }
// 11.40V is low for AGM batteries under load. Lights will be disabled
if (average <= 11.40) {
  BatLOW = true;
  }
if (average >= 11.50) {
  BatLOW = false;
  }
// ----------------------------------- Blinker Patterns -------------------------------------------------------
// Blink left & right simultaneously if Blinker Button is pressed once
   if (Blinkerstate == 1) {
    if (currentMillis - BlinkerpreviousMillis >= BlinkerInterval) {
    BlinkerpreviousMillis = currentMillis;
      if (RelBlinkerLinks13state == LOW) {
        RelBlinkerLinks13state = HIGH;
      } else {
        RelBlinkerLinks13state = LOW;
        }
      if (RelBlinkerRechts14state == LOW) {
        RelBlinkerRechts14state = HIGH;
      } else {
        RelBlinkerRechts14state = LOW;
        }
     digitalWrite(RelBlinkerLinks13, RelBlinkerLinks13state);
     digitalWrite(RelBlinkerRechts14, RelBlinkerRechts14state);
   }
  }
// Blink left & right alternating if Blinker Button is pressed twice
   if (Blinkerstate == 2) {
    if (currentMillis - BlinkerpreviousMillis >= BlinkerInterval) {
    BlinkerpreviousMillis = currentMillis;

      if (RelBlinkerLinks13state == LOW) {
        RelBlinkerLinks13state = HIGH;
      } else {
        RelBlinkerLinks13state = LOW;
        }
      if (RelBlinkerLinks13state == LOW) {
        RelBlinkerRechts14state = HIGH;
      } else {
        RelBlinkerRechts14state = LOW;
        }
     digitalWrite(RelBlinkerLinks13, RelBlinkerLinks13state);
     digitalWrite(RelBlinkerRechts14, RelBlinkerRechts14state);
   }
  }
  // Turn on Blinker left & right permanently if pressed 3 times
   if (Blinkerstate == 4) {
     digitalWrite(RelBlinkerLinks13, HIGH);
     digitalWrite(RelBlinkerRechts14, HIGH);
   }
  // Turn off Blinker
   if (Blinkerstate == 0) {
     digitalWrite(RelBlinkerLinks13, LOW);
     digitalWrite(RelBlinkerRechts14, LOW);
     RelBlinkerLinks13state = LOW;
     RelBlinkerRechts14state = LOW;
   }
// --------------------------------------- Zuendung ON & Park Brake control ---------------------------------------------------
  // Enable all functions - Turn on  Status LED if Zuendung is on.
  if ( switchzuendung.isPressed() == true ) {
       // Feststellbremse - (works also if Battery is low)
       if ( switchfeststellbremse.isPressed() == true ) {
         if ( sensorfbremsegezogen.isPressed() == false ) {
          digitalWrite(Out1Motor18, HIGH);
          digitalWrite(Out2Motor19, LOW);
          // Fast Blink FBremse LED while working
         if (currentMillis - FBlinkerpreviousMillis >= FBlinkerInterval) {
          FBlinkerpreviousMillis = currentMillis;
          digitalWrite(ledFBremse9, HIGH);
          FlashpreviousMillis = currentMillis;
         }
         if (currentMillis - FlashpreviousMillis >= FlashInterval) {
           digitalWrite(ledFBremse9, LOW);
         }
         } else {
           digitalWrite(Out1Motor18, LOW);
           digitalWrite(Out2Motor19, LOW);
           digitalWrite(ledFBremse9, HIGH);
           }
       }
       if ( switchfeststellbremse.isPressed() == false ) {
         if ( sensorfbremsegeloest.isPressed() == false ) {
          digitalWrite(Out1Motor18, LOW);
          digitalWrite(Out2Motor19, HIGH);
          // Fast Blink FBremse LED while motor is working
         if (currentMillis - FBlinkerpreviousMillis >= FBlinkerInterval) {
          FBlinkerpreviousMillis = currentMillis;
          digitalWrite(ledFBremse9, HIGH);
          FlashpreviousMillis = currentMillis;
         }
          if (currentMillis - FlashpreviousMillis >= FlashInterval) {
           digitalWrite(ledFBremse9, LOW);
          }
         } else {
           digitalWrite(Out1Motor18, LOW);
           digitalWrite(Out2Motor19, LOW);
           digitalWrite(ledFBremse9, LOW);
           }
       }
    // --------------------------------------- LIGHT & Horn Control ---------------------------------------------------
    // Lights are only enabled if Battery is not LOW
    if (BatLOW == false && BatidleLOW == false){
      // Startup lighting sequence
      if (startup == true) {
       // Turn on Standlicht permanently
       digitalWrite(RelStandlicht10, HIGH);
       digitalWrite(RelBlinkerLinks13, HIGH);
       digitalWrite(RelBlinkerRechts14, HIGH);
       delay (500);
       digitalWrite(RelBlinkerLinks13, LOW);
       digitalWrite(RelBlinkerRechts14, LOW);
       digitalWrite(RelAbblendlicht11, HIGH);
       digitalWrite(RelBremslicht15, HIGH);
       delay (500);
       digitalWrite(RelAbblendlicht11, LOW);
       digitalWrite(RelBremslicht15, LOW);
       digitalWrite(RelBlinkerLinks13, HIGH);
       digitalWrite(RelBlinkerRechts14, HIGH);
       delay (500);
       digitalWrite(RelBlinkerLinks13, LOW);
       digitalWrite(RelBlinkerRechts14, LOW);
       startup = false;
      }
       // Turn on Status LED
       digitalWrite(ledStatus8, HIGH);
       // Abblendlicht
       if ( switchabblendlicht.isPressed() == true ) {
       digitalWrite(RelAbblendlicht11, HIGH);
       } else {
       digitalWrite(RelAbblendlicht11, LOW);
       }
       // Fernlicht
       if ( switchfernlicht.isPressed() == true ) {
       digitalWrite(RelFernlicht12, HIGH);
       } else {
       digitalWrite(RelFernlicht12, LOW);
       }
       // Bremslicht
       if ( sensorbremse.isPressed() == true ) {
       digitalWrite(RelBremslicht15, HIGH);
       } else {
       digitalWrite(RelBremslicht15, LOW);
       }
       // Horn
       if ( buttonhorn.isPressed() == true ) {
       digitalWrite(RelHorn16, HIGH);
       } else {
       digitalWrite(RelHorn16, LOW);
       }
       // Blinker State
       if ( buttonBlinker.isClick() ) {
          Blinkerstate = Blinkerstate + 1;
          if (Blinkerstate > 3){
             Blinkerstate = 0;
          }
        }
       // Blinker off
       if ( buttonBlinker.isDoubleClick() ) {
             Blinkerstate = 0;
        }
    }
    // ------------------------------------- BATTERY Safe mode -----------------------------------------------------
    if (BatLOW == true) {
       // Turn off Blinker
       Blinkerstate = 0;
       // Turn off all Lights
      digitalWrite(RelStandlicht10, LOW);
      digitalWrite(RelAbblendlicht11, LOW);
      digitalWrite(RelFernlicht12, LOW);
      digitalWrite(RelBlinkerLinks13, LOW);
      digitalWrite(RelBlinkerRechts14, LOW);
      digitalWrite(RelBremslicht15, LOW);
      digitalWrite(RelHorn16, LOW);
      // Blink ledStatus8 fast if Battery is low and Zuendung is on
       if (currentMillis - statusledpreviousMillis >= FBlinkerInterval) {
        statusledpreviousMillis = currentMillis;
        digitalWrite(ledStatus8, HIGH);
        statusledflashpreviousMillis = currentMillis;
       }
       if (currentMillis - statusledflashpreviousMillis >= FlashInterval) {
         digitalWrite(ledStatus8, LOW);
       }
    }
  } else {
    // ---------------------------------------- SHUTDOWN activities --------------------------------------------------------
    // Turn off Blinker
    Blinkerstate = 0;
    // Turn off all outputs
    digitalWrite(ledFBremse9, LOW);
    digitalWrite(Out1Motor18, LOW);
    digitalWrite(Out2Motor19, LOW);
    digitalWrite(RelStandlicht10, LOW);
    digitalWrite(RelAbblendlicht11, LOW);
    digitalWrite(RelFernlicht12, LOW);
    digitalWrite(RelBlinkerLinks13, LOW);
    digitalWrite(RelBlinkerRechts14, LOW);
    digitalWrite(RelBremslicht15, LOW);
    digitalWrite(RelHorn16, LOW);
    // Goodbye - Blink twice
    if (startup == false) {
      if (BatLOW == false) {
       delay (300);
       digitalWrite(RelBlinkerLinks13, HIGH);
       digitalWrite(RelBlinkerRechts14, HIGH);
       delay (300);
       digitalWrite(RelBlinkerLinks13, LOW);
       digitalWrite(RelBlinkerRechts14, LOW);
       delay (200);
       digitalWrite(RelBlinkerLinks13, HIGH);
       digitalWrite(RelBlinkerRechts14, HIGH);
       delay (300);
       digitalWrite(RelBlinkerLinks13, LOW);
       digitalWrite(RelBlinkerRechts14, LOW);
      }
     }
    // Reset startup state to re-enable startup sequence during next ignition.
    startup = true;
    // ----------------------------- IDLE Activities -------------------------------------------------------------
    // Blink Status LED when Zuendung is turned off. & Blink FBremse LED when fbremse is active.
    if (currentMillis - BlinkerpreviousMillis >= BlinkerInterval) {
        BlinkerpreviousMillis = currentMillis;
        digitalWrite(ledStatus8, HIGH);
        if (sensorfbremsegezogen.isPressed() == true) {
           digitalWrite(ledFBremse9, HIGH);
        }
        FlashpreviousMillis = currentMillis;
    }
    if (currentMillis - FlashpreviousMillis >= FlashInterval) {
       digitalWrite(ledStatus8, LOW);
           if (sensorfbremsegezogen.isPressed() == true) {
               digitalWrite(ledFBremse9, LOW);
          }
     }
  }
}