#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].rose();
	Stw_data.Stw_shift_down = debouncer[4].rose();
	Stw_data.Stw_shift_up	= debouncer[5].rose();

	Stw_data.buttonState1 = digitalRead(button1);
	Stw_data.buttonState4 = digitalRead(button4);
	Stw_data.buttonStateEnc1 = digitalRead(enc1PinS);
	Stw_data.buttonStateEnc2 = digitalRead(enc2PinS);
	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);  
	}*/
}