#include "Arduino.h"
#include "FT18e_STW_INIT.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};
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(){
	pinMode (l,OUTPUT);
	for (int thisLed = 0; thisLed < sizeof(led)/sizeof(int); thisLed++) {
		pinMode(led[thisLed], 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 < 8; i++){
		pinMode(buttons[i], INPUT);
		debouncer[i].attach(buttons[i]);
		debouncer[i].interval(10);
	}
}	

void read_buttons(){
	/*entprell = digitalRead(button3);
	delay(10);
	if(digitalRead(button3)){
		Stw_data.Stw_auto_shift = entprell;
	}
	entprell = digitalRead(button2);
	delay(10);
	if(digitalRead(button2)){
		Stw_data.Stw_neutral = entprell;
	}
	entprell = digitalRead(button1);
	delay(10);
	if(digitalRead(button1)){
		Stw_data.buttonState1 = entprell;
	}
	entprell = digitalRead(button6);
	delay(10);
	if(digitalRead(button6)){
		Stw_data.Stw_shift_up = entprell;
	}
	entprell = digitalRead(button5);
	delay(10);
	if(digitalRead(button5)){
		Stw_data.Stw_shift_down = entprell;
	}
	entprell = digitalRead(button4);
	delay(10);
	if(digitalRead(button4)){
		Stw_data.buttonState4 = entprell;
	}
	entprell = digitalRead(enc1PinS);
	delay(10);
	if(digitalRead(enc1PinS)){
		Stw_data.buttonStateEnc1 = entprell;
	}
	entprell = digitalRead(enc2PinS);
	delay(10);
	if(digitalRead(enc2PinS)){
		Stw_data.buttonStateEnc2 = entprell;
	}*/
	Stw_data.Stw_auto_shift = digitalRead(button3);
	Stw_data.Stw_neutral = digitalRead(button2);
	Stw_data.buttonState1 = digitalRead(button1);
	Stw_data.Stw_shift_up	= digitalRead(button6);
	Stw_data.Stw_shift_down = digitalRead(button5);
	Stw_data.buttonState4 = digitalRead(button4);
	Stw_data.buttonStateEnc1 = digitalRead(enc1PinS);
	Stw_data.buttonStateEnc2 = digitalRead(enc2PinS);
	
	/*for(int i = 0;i < 8; i++){
		debouncer[i].update();
	}
	debouncer[2].update();
	if(debouncer[2].fell()){
		Stw_data.Stw_auto_shift = HIGH;
	}
	if(debouncer[1].fell()){
		Stw_data.Stw_neutral = digitalRead(button2);
	}
	if(debouncer[0].fell()){
		Stw_data.buttonState1 = digitalRead(button1);
	}
	if(debouncer[5].fell()){
		Stw_data.Stw_shift_up	= digitalRead(button6);
	}
	if(debouncer[4].fell()){
		Stw_data.Stw_shift_down = digitalRead(button5);
	}
	if(debouncer[3].fell()){
		Stw_data.buttonState4 = digitalRead(button4);
	}
	if(debouncer[6].fell()){
		Stw_data.buttonStateEnc1 = digitalRead(enc1PinS);
	}
	if(debouncer[7].fell()){
		Stw_data.buttonStateEnc2 = digitalRead(enc2PinS);
	}*/
 }
	
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);  
	}*/
}