#include "system_sam3x.h"
#include "at91sam3x8.h"
#include "display.h"
#include "include.c"


/* 
* KEYPAD INTIERING
* Initerar pinsen
*/

void key_init(){
	*AT91C_PIOC_PER = 0x3BC;                         // Enabla pin 34-37 och 39-41
	*AT91C_PIOD_PER = AT91C_PIO_PD2;                 //Enable pin på PD2
	*AT91C_PIOD_OER = AT91C_PIO_PD2;                 //Sätter den till output enable

	*AT91C_PIOD_CODR = AT91C_PIO_PD2;                //Clerar PD2
	*AT91C_PIOC_OER = 0x3BC;                         //Sätter pin34-37 och 39-41 till output.
	*AT91C_PIOC_SODR = 0x380;        

	*AT91C_PIOC_ODR = 0x3C;
}

/* 
* KEYPAD PRESSED
* Retunerar värdet från keypaden
*/

int key_pressed(){
        key_init();
        
  	int value = 0;
        int col[] ={0x100, 0x200, 0x80};     //Kolumnenrna på keypaden
	int row[] ={0x20, 0x4, 0x8, 0x10};    //Raderna på keypaden
        
	for(int i=0; i<3 && value==0; i++){   //Loopar igenom och kollar vilken knapp som är itryckt
		*AT91C_PIOC_CODR = col[i];
		for(int j=0; j<4 && value ==0 ; j++){
			if((*AT91C_PIOC_PDSR & row[j]) == 0){
				value = j*3 + i+1;
			}
		}
		*AT91C_PIOC_SODR = col[i];
	}
	*AT91C_PIOC_ODR = 0x380;
	*AT91C_PIOD_SODR = AT91C_PIO_PD2;

	return(value);
}

/* 
* KEYPAD UTSKRIFT
* Skriver ut knapptrycket på LCDn
* Ex för användning
* print_key(key_pressed());
*/

void print_key(int value){          //Printar keypaden
	if(value != 0){
		if(value == 10){                //Skriver om 10 till *
			Write_Data_2_Display(0x0A);
			Write_Command_2_Display(0xC0);
		}
		if(value == 11){               //Skriver om 11 till 0an
			Write_Data_2_Display(0x10);
			Write_Command_2_Display(0xC0);
		}
		if(value == 12){                //Skriver om 12 till #
			Write_Data_2_Display(0x03);
			Write_Command_2_Display(0xC0);
		}
		else if(value < 10){            //Tar input värden och adderar 0x10 för att få rätt värde
			int temp = value + 0x10;
                        Write_Data_2_Display(1);
                        Write_Data_2_Display(0);
                        Write_Command_2_Display(0x24);
			Write_Data_2_Display(temp);
			Write_Command_2_Display(0xC0);
		}
	}       
}

/* 
* KEYPAD SERVO-TEST
* Roterar servon
* Roterar åt höger med KEYPAD 1
* Roterar åt vänster med KEYPAD 3
* servo_test(key_pressed());
*/

void servo_test(int value) {
  if(value != 0){
    if (value == 1) {
      servo_setPos(servo_getPos() + 50);
    }
     if (value == 3) {
      servo_setPos(servo_getPos() - 50);
    }
    print_servo();
  }
}