#include "FT18e_STW_INIT.h"
#include "Arduino.h"
#include "Bounce2.h"
#include "RotaryEncoder.h"

volatile stw_data_type Stw_data = {0};         // alles mit 0 initialisieren
volatile vehicle_data_type Vehicle_data = {0}; // alles mit 0 initialisieren
bool enc1PinALast, enc1PinANow, enc2PinALast, enc2PinANow;
int led[] = {led1, led2,  led3,  led4,  led5,  led6,  led7,  led8,
             led9, led10, led11, led12, led13, led14, led15, led16};
bool entprell;
int buttons[] = {PIN_BUTTON_LL, PIN_BUTTON_LR, PIN_BUTTON_RL,
                 PIN_BUTTON_RR, enc1PinS,      enc2PinS};
Bounce debouncer[8];
double val = 0;
double val2 = 0;
RotaryEncoder encoder(enc1PinA, enc1PinB, 1, 1, 50);
RotaryEncoder encoder2(enc2PinA, enc2PinB, 1, 1, 50);
///////////////////////////////////////////////////
// functions
///////////////////////////////////////////////////

void set_pins() {
  for (int thisLed = 0; thisLed < sizeof(led) / sizeof(int); thisLed++) {
    pinMode(led[thisLed], OUTPUT);
  }
  pinMode(l, OUTPUT);
  /*pinMode(button1, INPUT);
  pinMode(button2, INPUT);
  pinMode(button3, INPUT);
  pinMode(button4, INPUT);
  pinMode(button5, INPUT);
  pinMode(button6, INPUT);*/
  pinMode(enc1PinA, INPUT);
  pinMode(enc1PinB, INPUT);
  // pinMode(enc1PinS, INPUT);
  pinMode(enc2PinA, INPUT);
  pinMode(enc2PinB, INPUT);
  // pinMode(enc2PinS, INPUT);
  // Stw_data.i=0;
  enc1PinALast = LOW;
  enc1PinANow = LOW;
  enc2PinALast = LOW;
  enc2PinANow = LOW;
  for (int i = 0; i < sizeof(buttons) / sizeof(*buttons); i++) {
    pinMode(buttons[i], INPUT);
    debouncer[i].attach(buttons[i]);
    debouncer[i].interval(10);
  }
}

void read_buttons() {
  Stw_data.button_ll = digitalRead(PIN_BUTTON_LL);
  Stw_data.button_lr = digitalRead(PIN_BUTTON_LR);
  Stw_data.button_rl = digitalRead(PIN_BUTTON_RL);
  Stw_data.button_rr = digitalRead(PIN_BUTTON_RR);
}

void read_rotary() {
  int enc2 = encoder2.readEncoder();
  if (enc2 != 0) {
    val2 = val2 + 0.5 * enc2;
    if (val2 == 1 or val2 == -1) {
      if ((Stw_data.mode == 1 or Stw_data.mode == 0) and enc2 < 0) {
        Stw_data.mode = 5;
      } else if (Stw_data.mode == 5 and enc2 > 0) {
        Stw_data.mode = 1;
      } else {
        Stw_data.mode = Stw_data.mode + enc2;
      }
      val2 = 0;
    }
  }
}