#include <stdlib.h>
#include <libraw1394/raw1394.h>
#include <libdc1394/dc1394_control.h>
#include <unistd.h>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <linux/fb.h>
#include <sys/mman.h>
#include <string.h>
#include <curses.h>

static dc1394_cameracapture cameras[4];
static int numNodes;
static int numCameras;
static raw1394handle_t handle;
static nodeid_t * camera_nodes;
static dc1394_feature_set features;

//int current_camera;
int camera_number;

int fbfd=0;
struct fb_var_screeninfo vinfo;
struct fb_fix_screeninfo finfo;
long int screensize=0;
char *fbp = 0;
int x=0,y=0;
long int location=0;
unsigned char* video;

void display(void)
{
	int key,no,tmp;
	
	do {
		key=getch();
	
	  	dc1394_dma_single_capture(&cameras[0]);

		memcpy(video,cameras[0].capture_buffer,640*480*3);
	
		for ( y = 0; y < 240; y++ )
			for ( x = 0; x < 320; x++ ) {
				location = (x+vinfo.xoffset) * (vinfo.bits_per_pixel/8)+(y+vinfo.yoffset) * finfo.line_length;
				tmp=x*3*2+y*1920*2;
				*(fbp+location+2) = *(video+tmp); 
				*(fbp+location+1) = *(video+tmp+1);
				*(fbp+location+0) = *(video+tmp+2); 
				*(fbp + location + 3) = 0;   
			}
	
		dc1394_dma_done_with_buffer(&cameras[0]);	

	  	dc1394_dma_single_capture(&cameras[1]);

		memcpy(video,cameras[1].capture_buffer,640*480*3);
	
		for ( y = 0; y < 240; y++ )
			for ( x = 0; x < 320; x++ ) {
				location = (x+vinfo.xoffset+320) * (vinfo.bits_per_pixel/8)+(y+vinfo.yoffset) * finfo.line_length;
				tmp=x*3*2+y*1920*2;
				*(fbp+location+2) = *(video+tmp); 
				*(fbp+location+1) = *(video+tmp+1);
				*(fbp+location+0) = *(video+tmp+2); 
				*(fbp + location + 3) = 0;   
			}
	
		dc1394_dma_done_with_buffer(&cameras[1]);	
	  	
		dc1394_dma_single_capture(&cameras[2]);

		memcpy(video,cameras[2].capture_buffer,640*480*3);
	
		for ( y = 0; y < 240; y++ )
			for ( x = 0; x < 320; x++ ) {
				location = (x+vinfo.xoffset) * (vinfo.bits_per_pixel/8)+(y+vinfo.yoffset+240) * finfo.line_length;
				tmp=x*3*2+y*1920*2;
				*(fbp+location+2) = *(video+tmp); 
				*(fbp+location+1) = *(video+tmp+1);
				*(fbp+location+0) = *(video+tmp+2); 
				*(fbp + location + 3) = 0;   
			}
	
		dc1394_dma_done_with_buffer(&cameras[2]);	
/*		
		no=key-49;
		if (no>=0 && no<camera_number)
			if (no!=current_camera) {
				dc1394_stop_iso_transmission(handle,cameras[current_camera].node);
				dc1394_start_iso_transmission(handle,cameras[no].node);
				current_camera=no;
			}
*/		
	} while (key!='q' && key!='Q');
}

int fbinit()
{
	fbfd=open("/dev/fb0",O_RDWR);
	if (!fbfd) {
		printf("failure : open framebuffer\n");
		exit(1);
	}

	// Get fixed screen information
        if (ioctl(fbfd, FBIOGET_FSCREENINFO, &finfo)) {
		printf("faliure : reading fixed info\n");
		exit(1);
	}

	// Get variable screen information
	if (ioctl(fbfd, FBIOGET_VSCREENINFO, &vinfo)) {
		printf("failure : reading varibale info\n");
		exit(1);
	}

	printf("%dx%d, %dbpp\n", vinfo.xres, vinfo.yres, vinfo.bits_per_pixel );

	screensize = vinfo.xres * vinfo.yres * vinfo.bits_per_pixel / 8;

	fbp = (char *)mmap(0, screensize, PROT_READ | PROT_WRITE, MAP_SHARED,
			fbfd, 0);
	if ((int)fbp==-1) {
		printf("failure : mmap\n");
		exit(1);
	}

	printf("fbinit finished\n");
}

int ieee1394init()
{
	quadlet_t formats;
	quadlet_t modes;
	quadlet_t framerates;
	int i;
	
	/*Open ohci and asign handle to it*/
	handle = dc1394_create_handle(0);
	/*get the camera nodes and describe them as we find them*/
	numNodes = raw1394_get_nodecount(handle);
	camera_nodes = dc1394_get_camera_nodes(handle,&numCameras,1);
	camera_number=numCameras;

	if (camera_nodes[0]==numNodes-1) {
		printf("Camera can't be highest node\n");
		exit(1);
	}
	
	for (i=0;i<numCameras;i++) {
		printf("Camera %d\n",i);
	dc1394_dma_setup_capture(handle,camera_nodes[i],
			i,
			FORMAT_VGA_NONCOMPRESSED,
			MODE_640x480_RGB,
			SPEED_400,
			FRAMERATE_7_5,
			4,
			&cameras[i]);
	//dc1394_get_camera_feature_set(handle, cameras[i].node,&features);
	//dc1394_print_feature_set(&features);

	dc1394_query_supported_formats(handle,cameras[i].node,&formats);
	if (formats & (0x1<<31)) {
		printf("VGA non-compressed format supported\n");
		dc1394_query_supported_modes(handle,cameras[i].node,FORMAT_VGA_NONCOMPRESSED,&modes);
	}
	if (modes & (0x1<<29))
		printf("640x480 YUV 4:1:1 supported\n");
	if (modes & (0x1<<28))
		printf("640x480 YUV 4:2:2 supported\n");
	if (modes & (0x1<<27))
		printf("640x480 RGB supported\n");
	if (modes & (0x1<<26))
		printf("640x480 Mono supported\n");

	printf("while 640x480 RGB :\n");
	
	dc1394_query_supported_framerates(handle,cameras[i].node,FORMAT_VGA_NONCOMPRESSED,MODE_640x480_RGB,&framerates);
	if (framerates & (0x1<<30))
		printf("3.75 fps supported\n");
	if (framerates & (0x1<<29))
		printf("7.5 fps supported\n");
	if (framerates & (0x1<<28))
		printf("15 fps supported\n");
	if (framerates & (0x1<<27))
		printf("30 fps supported\n");
	}
	
	dc1394_start_iso_transmission(handle,cameras[0].node);
	dc1394_start_iso_transmission(handle,cameras[1].node);
	dc1394_start_iso_transmission(handle,cameras[2].node);

	printf("ieee1394init finish\n");
}

int main(int argc, char** argv)
{
	int i;
	
	video=malloc(640*480*3);
	
	ieee1394init();
	
	fbinit();
	
	initscr();
	noecho();
	timeout(50);
//	nl();
//	cbreak();
//	nonl();
//	intrflush(stdscr,FALSE);
//	keypad(stdscr,TRUE);
	
	display();
	
	dc1394_stop_iso_transmission(handle,cameras[0].node);
	dc1394_stop_iso_transmission(handle,cameras[1].node);
	dc1394_stop_iso_transmission(handle,cameras[2].node);
   
	for (i=0;i<camera_number;i++) {
		dc1394_dma_release_camera(handle,&cameras[i]);
	   	dc1394_dma_unlisten(handle,&cameras[i]);
	}
		
	raw1394_destroy_handle(handle);
   
	munmap(fbp,screensize);
	close(fbfd);

	free(video);

	endwin();
	
	return 0;   
}