#include <p30f6014A.h>

//#define FLOOR_SENSORS	// define to enable floor sensors
//#define LIS_SENSORS_TURRET
#define IR_RECIEVER

#include <string.h>
#include <ctype.h>
#include <stdio.h>

#include <uart/e_uart_char.h>
#include <motor_led/e_init_port.h>
#include <bluetooth/e_bluetooth.h>
#include <motor_led/e_motors.h>

#define SELECTOR0 _RG6
#define SELECTOR1 _RG7
#define SELECTOR2 _RG8
#define SELECTOR3 _RG9

#define uart_send_static_text(msg) do { e_send_uart1_char(msg,sizeof(msg)-1); while(e_uart1_sending()); } while(0)
#define uart_send_text(msg) do { e_send_uart1_char(msg,strlen(msg)); while(e_uart1_sending()); } while(0)

// static char buffer[39*52*2+3+80];

struct BtDevice epuck[2];

char pin[16], name[50];
char send[100], read[100];
char c;
int selector;

void hw_init(void){
	// initialize UART to 115200 Kbaud
	e_init_uart1();   
	e_init_port();
	e_init_motors();
 	e_set_speed(0, 0);

/*	
	// Factory Reset
	e_bt_factory_reset();
	// Device Name & PIN
	e_bt_write_local_name("e-puck_2685");
	e_bt_write_local_pin_number("2685");
*/
	// Transparent Mode
	e_bt_tranparent_mode();	
}

void terminalIO(void){
	while (e_getchar_uart1(&c)==0);
	switch(c) {
		case 'T':
			uart_send_text("\f\a"
						   "Test-Modus\r\n");
			break;
		case 'P':
			e_bt_read_local_pin_number(pin);
			sprintf("\n\rPIN code: %s",pin);
			break;
		case 'N':
			e_bt_read_local_name(name);
			sprintf("\n\rFriendly name: %s",name);
			break;	
		case 'H':	
			uart_send_static_text("\f\a \"N\" Read actual Friendly Bluetooth Name\r\n");
			uart_send_static_text("\f\a \"P\" Read actual PIN Number\r\n"); 
			break;
	   	}
}

void BTconnect(void){
	// Connect to specific e-puck
	int i;
	int j = 0;
	int device_find = e_bt_inquiry(e_bt_present_device);

	for(i = 0; i < device_find; i++){
		e_bt_get_friendly_name(&e_bt_present_device[i]);
		
		if((e_bt_present_device[i].friendly_name[0]=='e')&
		   (e_bt_present_device[i].friendly_name[1]=='-')&
		   (e_bt_present_device[i].friendly_name[2]=='p')){
			epuck[j] = e_bt_present_device[i];
			e_bt_etablish_SPP_link(epuck[j].address);
			j++;
		}
	}
}

void sendData(char send[], char datalength){
	// Send Data
	e_bt_send_SPP_data(send, datalength);
}

char recieveData(void){
	// Recieve Data
	int i;

	do {
		if (e_getchar_uart1(&read[i])){
			c = read[i];
			i++;
		}
	} while (((char)c != 0x03)||(i<(read[3]+6)));
	
	e_set_speed(read, read);
}

/* \brief The main function of the programm */
int main(void) {
	hw_init();
	BTconnect();
	
	// Get Selector-Value
	selector = SELECTOR0 + 2*SELECTOR1 + 4*SELECTOR2 + 8*SELECTOR3;

	if(selector == 0){
		sendData("30", "2");
	} else if(selector == 1){
		recieveData();
	} else {
		uart_send_text("\r\nWrong Selector position");
	}

	while(1);
}