#include "FT18_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[] = {button1, button2, button3,  button4,
                 button5, button6, enc1PinS, enc2PinS};
constexpr size_t N_BUTTONS = sizeof(buttons) / sizeof(buttons[0]);
Bounce2::Button debouncer[N_BUTTONS];
double val = 0;
double val2 = 0;
RotaryEncoder encoder(enc1PinA, enc1PinB, 1, 1, 50);
RotaryEncoder encoder2(enc2PinA, enc2PinB, 1, 1, 50);
///////////////////////////////////////////////////
// functions
///////////////////////////////////////////////////

void set_pins() {
  pinMode(l, OUTPUT);
  for (int thisLed = 0; thisLed < sizeof(led) / sizeof(int); thisLed++) {
    pinMode(led[thisLed], OUTPUT);
  }
  pinMode(enc1PinA, INPUT);
  pinMode(enc1PinB, INPUT);
  pinMode(enc2PinA, INPUT);
  pinMode(enc2PinB, INPUT);
  enc1PinALast = LOW;
  enc1PinANow = LOW;
  enc2PinALast = LOW;
  enc2PinANow = LOW;
  for (int i = 0; i < N_BUTTONS; i++) {
    debouncer[i].attach(buttons[i], INPUT);
    debouncer[i].interval(10);
  }
}

void read_buttons() {
  for (int i = 0; i < N_BUTTONS; i++) {
    debouncer[i].update();
  }

  // These are only used to send them out via CAN, so they only need to be
  // high once.
  //Stw_data.Stw_neutral = debouncer[1].rose();
  //Stw_data.Stw_auto_shift = debouncer[2].isPressed(); //Hold to open DRS
  // 05.09.: swapped neutral and auto_shift (DRS)
  Stw_data.Stw_neutral = debouncer[2].rose();
  Stw_data.Stw_auto_shift = debouncer[1].isPressed(); //Hold to open DRS
  Stw_data.Stw_shift_down = debouncer[4].rose();
  Stw_data.Stw_shift_up = debouncer[5].rose();

  Stw_data.buttonState1 = debouncer[0].isPressed();
  Stw_data.buttonState4 = debouncer[3].isPressed();
  Stw_data.buttonStateEnc1 = debouncer[6].isPressed();
  Stw_data.buttonStateEnc2 = debouncer[7].isPressed();
  if (debouncer[0].rose()) {
    Stw_data.button1_rises++;
  }
  if (debouncer[3].rose()) {
    Stw_data.button4_rises++;
  }
  if (debouncer[6].rose()) {
    Stw_data.enc1_rises++;
  }
  if (debouncer[7].rose()) {
    Stw_data.enc2_rises++;
  }
}

void read_rotary() {
  int enc = encoder.readEncoder();
  int enc2 = encoder2.readEncoder();
  if (enc != 0) {
    val = val + 0.5 * enc;
    if (val == 1 or val == -1) {
      if (Stw_data.trc == 0 and enc < 0) {
        Stw_data.trc = 11;
      } else if (Stw_data.trc == 11 and enc > 0) {
        Stw_data.trc = 0;
      } else {
        Stw_data.trc = Stw_data.trc + enc;
      }
      val = 0;
    }
  }
  /*enc1PinANow = digitalRead(enc1PinA);
  enc2PinANow = digitalRead(enc2PinA);
  if ((enc1PinALast == LOW) && (enc1PinANow == HIGH)) {
          if (digitalRead(enc1PinB) == HIGH) {
                  if(Stw_data.trc==0){
                          Stw_data.trc = 5;
                  }else{
                          Stw_data.trc--;
                  }
          }else {
                  if(Stw_data.trc==5){
                          Stw_data.trc=0;
                  }else{
                          Stw_data.trc++;
                  }
          }
  }
  enc1PinALast = enc1PinANow;
  /*if (Stw_data.buttonStateEnc1 == HIGH){
          digitalWrite(led[Stw_data.i], HIGH);
  }*/
  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;
    }
  }
  /*if ((enc2PinALast == LOW) && (enc2PinANow == HIGH)) {
  //if(enc2PinALast != enc2PinANow){
          if (digitalRead(enc2PinB) == HIGH) {
                  if(Stw_data.i==0){
                          Stw_data.i = sizeof(led)/sizeof(int)-1;
                  }else{
                          Stw_data.i--;
                  }
          }else {
                  if(Stw_data.i==sizeof(led)/sizeof(int)-1){
                          Stw_data.i=0;
                  }else{
                          Stw_data.i++;
                  }
          }
  }
  enc2PinALast = enc2PinANow;*/
  /*if (Stw_data.buttonStateEnc2 == HIGH){
          digitalWrite(led[Stw_data.i], HIGH);
  }*/
}