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

#define EP_IN 0x81
#define EP_OUT 0x02

struct usb_dev_handle *aes3400;
struct usb_device *aes_device;

unsigned char image[128*128];
unsigned char rawdata[10000];

//USB Request Blocks for initialising the device
static unsigned char urb_1[4]={0x80,0x01,0x81,0x40};
static unsigned char urb_2[42]={0x80,0x01,0x82,0x40,0x83,0x00,0x88,0x02,0x89,0x10,0x8a,0x05,0x8c,0x00,0x8e,0x13,0x91,0x44,0x92,0x34,0x95,0x16,0x96,0x16,0x97,0x18,0xa1,0x70,0xa2,0x02,0xa7,0x00,0xac,0x01,0xad,0x1a,0x80,0x04,0x81,0x04,0xb4,0x00};
static unsigned char urb_3[54]={0x80,0x01,0x82,0x40,0x80,0x01,0x82,0x40,0x83,0x00,0x88,0x02,0x8c,0x7c,0x89,0x10,0x8d,0x24,0x9b,0x02,0x9c,0x09,0x9d,0x14,0x9e,0x33,0x9f,0x2f,0xa2,0x02,0xa7,0x00,0xb6,0x26,0xb7,0x1a,0x80,0x04,0x98,0x23,0x95,0x10,0x96,0x1f,0x8e,0x00,0x91,0x70,0x92,0x20,0x81,0x04,0xb4,0x00};
static unsigned char urb_4[14]={0x98,0x23,0x95,0x10,0x96,0x1f,0x8e,0x03,0x91,0x70,0x92,0x20,0x81,0x04};

static unsigned char urb_request_read[48]={0x8a, 0x05, 0x89, 0x02, 0x90, 0x00, 0x8c, 0x2a, 0x8d, 0x00, 0x91, 0x51, 0x92, 0x0e, 0x8e, 0x02, 0x8e, 0x02, 0x98, 0x22, 0x93, 0x00, 0x94, 0x07, 0x95, 0x00, 0x96, 0x1f, 0xa9, 0x40, 0x9b, 0x08, 0x9c, 0x3b, 0x9d, 0x35, 0x9e, 0x70, 0x9f, 0x75, 0x81, 0x00, 0x81, 0x04, 0x81, 0x00, 0x99, 0x04};

int tx_status, rx_length;

//------------------------------------------------------------------------------

void setup_image() {
	//initialise image to black
	int i;
	for (i=0;i<=128*128;i++){
		image[i]=(unsigned char)0x00;
	}
}

//------------------------------------------------------------------------------

int finger_present() {
	//detect a finger on the sensor
	int i=0, blackcount=0;
	for (i=0;i<=128*128;i++){
		if (image[i] == (unsigned char)0x00)
			blackcount++;
	}
	if (blackcount >= 128*64){
		return 0;
	}else{
		return 1;
	}
}

//------------------------------------------------------------------------------

void build_image(unsigned char *stream) {
	//build a raw image from the received data
	int i = 0;
	int row,col,byte;
	unsigned char pixel,pixA,pixB;
	for(col=0;col<8;col++){
		for(row=0;row<128;row++){
			for (byte=0;byte<16;byte=byte+2) {
				if(stream[i] >= (unsigned char)0xE0 && stream[i] <= (unsigned char)0xE7 && row==0){
					///printf("Header %02x found at %d\n",stream[i],i);
					i++;
				}
				pixel = stream[i++];
				pixA=pixel << 4;
				pixA=pixA  >> 4;
				pixB=pixel >> 4;
				image[(byte+1)+(128*row)+(col*16)] = pixB;
				image[(byte)+(128*row)+(col*16)] = pixA;
			}
		}
	}
}

//------------------------------------------------------------------------------

void save_image(void) {
	//save built image to a file
	printf("\nSaving finger.pnm... ");
	int x;
	FILE *imagefile = fopen("finger.pnm","w");
	fprintf(imagefile,"P2\n128 128\n16\n");
		for(x = 0; x < 128*128; x++) {
			fprintf(imagefile,"%d ",(unsigned char)image[x]);
		}
	fclose(imagefile);
	printf("saved.\n");
}

//------------------------------------------------------------------------------

void aquire_data(void) {
	//get raw data from device
	setup_image();
	printf("Waiting for finger..."); fflush(stdout);
	while(1){
		tx_status = usb_bulk_write(aes3400,EP_OUT,urb_request_read,sizeof(urb_request_read),500);	//request data
		rx_length = usb_bulk_read(aes3400, EP_IN, rawdata, 10000, 500);					//receive data
	 	if (rx_length == 8362){
		 	build_image(rawdata);
		 	if (finger_present()){
				printf("ok\n");
				break;
		 	}
	 	}
 		sleep(0.1);
	}
}

//------------------------------------------------------------------------------

usb_dev_handle *find_device(void){
	//find where reader is plugged in
	struct usb_bus *bus;
	struct usb_device *device;
	usb_dev_handle *device_handle = 0;
	usb_find_busses();
	usb_find_devices();
 	for (bus=usb_busses; bus; bus=bus->next){
		for (device=bus->devices; device; device=device->next)	{
			if ((device->descriptor.idVendor == 0x08ff)&&(device->descriptor.idProduct == 0x5731)){
				device_handle = usb_open(device);
				printf("Authentec AES3400 found at %s \n", device->filename);
			}
		}
	}
	if (device_handle==0) return(0);
	else return(device_handle);
}

//------------------------------------------------------------------------------

void config_device(void) {
	//configure device for communication
	if ((aes3400 = find_device())==0){
		printf("Device not found.\n");
		exit(EXIT_FAILURE);
	}
	if (tx_status=usb_set_configuration(aes3400,1) != 0){
		printf("ERROR: usb_set_configuration (code:%d)\n",tx_status);
		exit(EXIT_FAILURE);
	}
	if (tx_status=usb_claim_interface(aes3400,0) != 0){
		printf("ERROR: Device is busy (code:%d)\n",tx_status);
		exit(EXIT_FAILURE);
	}
	if (tx_status=usb_set_altinterface(aes3400,0) != 0){
		printf("ERROR: usb_set_altinterface (code:%d)\n",tx_status);
		exit(EXIT_FAILURE);
	}
}

//------------------------------------------------------------------------------

void init_device(void) {
tx_status = usb_bulk_write(aes3400,EP_OUT,urb_1,sizeof(urb_1),500);
rx_length = usb_bulk_read(aes3400, EP_IN, rawdata, 10000, 500); usleep(10000);

tx_status = usb_bulk_write(aes3400,EP_OUT,urb_2,sizeof(urb_2),500);
rx_length = usb_bulk_read(aes3400, EP_IN, rawdata, 10000, 500); usleep(10000);

tx_status = usb_bulk_write(aes3400,EP_OUT,urb_3,sizeof(urb_3),500);
rx_length = usb_bulk_read(aes3400, EP_IN, rawdata, 10000, 500); usleep(10000);

tx_status = usb_bulk_write(aes3400,EP_OUT,urb_4,sizeof(urb_4),500);
rx_length = usb_bulk_read(aes3400, EP_IN, rawdata, 10000, 500); usleep(10000);
}

//------------------------------------------------------------------------------

int main(void){
	//main program entry point
	usb_init();
	usb_set_debug(0);
	config_device();
	init_device();
	aquire_data();
	save_image();
	usb_close(aes3400);
}