diff --git a/firmware/lib/bughal.c b/firmware/lib/bughal.c
index 0eadfcd..dcff0cf 100644
--- a/firmware/lib/bughal.c
+++ b/firmware/lib/bughal.c
@@ -1,12 +1,24 @@
 #include <inttypes.h>
 #include <avr/io.h>
 #include "bughal.h"
+#include "util.h" //for timer
 
 /* Hardware abstraction layer for Pentabug hardware */
 
 /*
  * initialize LEDs on C0-C3
  */ 
+
+
+static uint8_t oldinput; // button readings from last poll cycle
+static uint8_t curinput; // button readings from current poll cycle
+//each switch has its own state machine
+static uint8_t btnstates[BTN_BUTTONS]; //array for current button states
+static uint8_t btncounters[BTN_BUTTONS]; //individual counter for button state machine
+static timer_t btntimers[BTN_BUTTONS];  //individiual timer for for button state machine
+
+
+
 void init_leds(void){
 	//enable LED channels as output
 	DDRC |= (1 << PORTC0) | (1 << PORTC1) | (1 << PORTC2) | (1 << PORTC3);
@@ -67,6 +79,15 @@ void buzzr_inv(void){
 void init_switch(void){
 	DDRD &= ~( (1 << PORTD1) | (1<<PORTD0));
 	PORTD |= (1 << PORTD1) | (1<<PORTD0); //Pullups FTW
+	// set predefined buttonstates
+	oldinput = 0;
+	curinput = 0;
+	for(uint8_t i=0; i<BTN_BUTTONS; i++){
+		btnstates[i] = BTNST_NTRL; //init button states
+		btncounters[i] = 0;        //init button counters
+		timer_set(&btntimers[i], 0x05); //50ms - init button timers
+	}
+
 	return;
 }
 
@@ -92,3 +113,99 @@ void set_motor(int val){
 	return;
 }
 
+// reset button to neutral state
+void button_clear(uint8_t button){
+	btnstates[button] = BTNST_NTRL;
+}
+
+
+void stateswitch(uint8_t i ){
+	switch(btnstates[i])
+	{
+		case BTNST_NTRL: //NEUTRAL
+			if (curinput & (1<<i)){ //button down
+				btncounters[i] = 0;
+				btnstates[i] = BTNST_DBNC;
+			}
+			break;
+		//intermediate state, check if button is still pressed to debounce
+		case BTNST_DBNC: 
+			btnstates[i] = (curinput & (1<<i))? BTNST_SDN: BTNST_NTRL;
+			(btncounters[i])++;
+			break;
+
+		case BTNST_SDN: //is shortpressed and still held down
+			if (curinput & (1<<i)){
+				btncounters[i]++;
+				if (btncounters[i] > BTN_T_LONGFACT){ //500ms held
+					btnstates[i] = BTNST_LDN;
+				}
+			} else { //button was released
+				btnstates[i] = BTNST_SUP;
+				//signal shortclick
+			}
+			break;
+		case BTNST_LDN: //is longpressed and still held down
+			if (!(curinput & (1<<i))){
+				//button was released
+				btnstates[i] = BTNST_LUP; //signal longpress
+			}
+			break;
+		case BTNST_SUP: //Button came up after being pressed shortly
+			if ((curinput & (1<<i))){
+				//button was pressed again or is bouncing after release
+				btnstates[i] = BTNST_SUPDBNC; //going in special debounce
+				btncounters[i] = 0;
+			}
+			break;
+		case BTNST_LUP: //Button came up after being pressed for a long time
+			if ((curinput & (1<<i))){
+				//button was pressed again or is bouncing after release
+				btnstates[i] = BTNST_LUPDBNC; //going in special debounce
+				btncounters[i] = 0;
+			}
+			break;
+		case BTNST_SUPDBNC: //Button was pressed again after beeing short pressed(or is bouncing)
+			if ((curinput & (1<<i))){
+				//button is still pressed --> going to shortpress
+				btncounters[i]++;
+				btnstates[i] = BTNST_SDN; //starting over from short pressed
+			} else {
+				btnstates[i] = BTNST_SUP; //nope, it was bouncing, back to old state
+			}
+			break;
+		case BTNST_LUPDBNC: //Button was pressed again after beeing short pressed(or is bouncing)
+			if ((curinput & (1<<i))){
+				//button is still pressed --> going to shortpress
+				btncounters[i]++;
+				btnstates[i] = BTNST_SDN; //starting over from short pressed
+			} else {
+				btnstates[i] = BTNST_LUP; //nope, it was bouncing, back to old state
+			}
+			break;
+		default: //curently catches nothing
+			// do nothing yet
+			;
+	} //end switch
+	timer_set(&btntimers[i], BTN_T_DEBOUNCE);
+}
+
+
+
+void button_poll(){
+	curinput = ~(PIND & 0b00000011);
+	for(uint8_t i=0; i<BTN_BUTTONS; i++){
+		if (timer_expired(&btntimers[i])){
+			stateswitch(i);
+		} //end if timer expired
+	} //end for
+	oldinput = curinput;
+	return;
+}
+
+bool btn_state(uint8_t btnstate, uint8_t btn){
+	return (btnstates[btn]==btnstate);
+}
+
+
+
diff --git a/firmware/lib/bughal.h b/firmware/lib/bughal.h
index bf9ef81..d3d875f 100644
--- a/firmware/lib/bughal.h
+++ b/firmware/lib/bughal.h
@@ -14,6 +14,32 @@ enum {  BUZZR_OUT,  //initialize buzzer for OUTPUT mode (emmiting soundwaves)
 #define MOTOR_ON (0<<PORTB1)
 #define MOTOR_OFF (1<<PORTB1)
 
+#define BTN_RIGHT		1
+#define BTN_LEFT		0
+#define BTN_BUTTONS     2  //numer of Buttons
+#define BTN_T_DEBOUNCE  5  // 50ms debounce time = minimum short press time
+#define BTN_T_LONGFACT  10 // after 10 * T_DEBOUNCE = 500ms button registers as long pressed
+
+//BUTTON state machine states
+
+#define BTNST_NTRL	0  // neutral - initial state nothing interesting, please go along
+
+#define BTNST_DBNC	1  // debounce - pin went up, but we wait for things to stabilize and stop oscillating
+
+#define BTNST_SDN 	4  // affirmative, button is pressed,
+			   // and it's pressed no longer than
+			   // BTN_T_LONGFACT * BTN_T_DEBOUNCE * 10ms
+
+#define BTNST_SUP	8  // and button went up after beeing pressed for a _short_ time
+
+#define BTNST_LDN  	16  // button is still down for more than
+			   //BTN_T_LONGFACT * BTN_T_DEBOUNCE * 10ms
+
+#define BTNST_LUP   	32  // button came up after being pressed for a long time
+
+#define BTNST_SUPDBNC 64 // debounce after short up
+#define BTNST_LUPDBNC 128 // debounce after long up
+
 void init_leds(void);
 void led_on(int);
 void led_off(int);
@@ -24,7 +50,10 @@ void buzzr_down(void);
 void buzzr_off(void);
 void buzzr_inv(void);
 
+// init ports and stuff for button functions
 void init_switch(void);
+
+// direct low level access to switches
 bool switch_l(void); //switch pressed?
 bool switch_r(void); //switch pressed?
 
@@ -32,6 +61,15 @@ void init_motor(void);
 void set_motor(int);
 
 
+// higher level functions for accessing switches
+
+void button_poll(void); //needs to be polled in regular intervalls (lets say every 10ms)
+
+// reset button to neutral state
+void button_clear(uint8_t button);
+
+//test if buttonstate of btn eqals btnstate, returns true if yes
+bool btn_state(uint8_t btnstate, uint8_t btn);
 
 
 #endif
diff --git a/firmware/main.c b/firmware/main.c
index c883aa5..4186645 100644
--- a/firmware/main.c
+++ b/firmware/main.c
@@ -6,7 +6,7 @@
 #include <stdlib.h>
 
 #include "main.h"
-#include "lib/synth.h"
+
 #include "lib/usart.h"
 #include "lib/bughal.h"
 #include "lib/util.h"
@@ -27,19 +27,45 @@ main(void)
 	
 	/* here the show begins:*/
 	sei();
-	//set_motor(MOTOR_ON);
 	timer_t t;
-	uint8_t ledmode =0;
-	uint16_t ct =0;
-	led_on(LED_R);
+	uint8_t ledstate_l =0;
+	uint8_t ledstate_r =0;
+	uint8_t motorstate =0;
 	timer_set(&t, 100);
 	for(;;) /* ever */  {
 	//do something
 	//main polling loop;
+		button_poll();
 		if (timer_expired(&t)){
-			if (switch_l()) {led_on(LED_L);} else {led_off(LED_L);};
-			if (switch_r()) {led_on(LED_R);} else {led_off(LED_R);};
-			if (switch_l()&&switch_r()) {set_motor(MOTOR_ON);} else {set_motor(MOTOR_OFF);};			
+			//while left button is pressed switch left led on
+			if (btn_state(BTNST_SDN,BTN_LEFT)|btn_state(BTNST_LDN,BTN_LEFT)){
+			   led_on(LED_L);
+                        } else {
+                           led_off(LED_L);
+                        };
+			//when right button has been pressed short, toggle right led
+			if (btn_state(BTNST_SUP,BTN_RIGHT)){
+			   if (ledstate_r ==0){
+				ledstate_r = 1;
+				led_on(LED_R);
+			   } else {
+				ledstate_r = 0;
+				led_off(LED_R);
+			   };
+		           button_clear(BTN_RIGHT); //this is important, to show the event has been processed
+			};	
+			//when right button has been pressed long, toggle motor
+			if (btn_state(BTNST_LUP,BTN_RIGHT)){
+			   if (motorstate ==0){
+				motorstate = 1;
+				set_motor(MOTOR_ON);
+			   } else {
+				motorstate = 0;
+				set_motor(MOTOR_OFF);
+			   };
+		           button_clear(BTN_RIGHT); //this is important, to show the event has been processed
+			};				
+			
 			timer_set(&t, 5);
 		}; //end if timer expired
 		//USART0_put_uint16(0xA09F);