#include <X11/Xlib.h>
#include <X11/extensions/XShm.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <X11/extensions/Xvlib.h>
#include <GL/glx.h>
#include <string.h>
#include <stdio.h>
#include <unistd.h>

#define FOURCC_I420		0x30323449U
#define FOURCC_IYUV		0x56555949U

using namespace std;

static bool use_shared = true;
static bool use_tfp = true;
static bool use_xv = false;
static bool want_opengl = true;

void (*glXBindTexImageEXT)(Display *dpy, GLXDrawable drawable, int buffer, const int *attrib_list);
void (*glXReleaseTexImageEXT)(Display *dpy, GLXDrawable drawable, int buffer);

static struct s_x_shm_returns {
  int major, minor;
  Bool pixmaps;
} x_shm_returns;

void
dump_xv_adaptor_infos(XvAdaptorInfo *infos, unsigned int ninfos) {
	for(int i = 0; i < ninfos; i++) {
		printf("Adaptor %02d: %s\n", i, (infos->name != NULL)?infos->name:"<Unnamed>");
		printf("\tPort: %02d Type: %d\n", infos->base_id, infos->type);
		printf("\tSupported Formats: %d\n", infos->num_formats);
		for(int j = 0; j < infos->num_formats; j++) {
			printf("\t\tDepth: %d, Visual: %d\n", infos->formats[j].depth, infos->formats[j].visual_id);
		}
		break;
		infos--;
	}
}

void
dump_xv_imgfmt_infos(XvImageFormatValues *imgfmts, unsigned int nimgfmts) {
	for(unsigned int i = 0; i < nimgfmts; i++) {
		printf("Format 0x%08x:\n", imgfmts[i].id);
		printf("\tType: %s\n", imgfmts[i].type == XvRGB ? "RGB" : (imgfmts[i].type == XvYUV)?"YUV":"Unknown");
	}
}

GLXFBConfig *get_suitable_fbconfig(Display *dpy, Window wnd, bool rgba) {
	XWindowAttributes xwattr;
	GLXFBConfig *fbconfigs;
	VisualID xvisual;
	int nfbconfigs, i = 0;

	/* First, get the attributes about the current X11 window. */
	XGetWindowAttributes(dpy, wnd, &xwattr);

	/* Get the visual from the attributes. */
	xvisual = XVisualIDFromVisual(xwattr.visual);

	/* Get the list of available FBConfig structures. */
	fbconfigs = glXGetFBConfigs(dpy, 0, &nfbconfigs);

	for(; i < nfbconfigs; i++) {
		int value;
		XVisualInfo *visinfo;

		/* Verfiy that the FBConfig and the current window have matching visuals. */
		visinfo = glXGetVisualFromFBConfig(dpy, fbconfigs[i]);
		if((visinfo == NULL) || (visinfo->visualid != xvisual))
			continue;

		/* Get the info on bind-to-texture targets support. */
		glXGetFBConfigAttrib(dpy, fbconfigs[i], GLX_BIND_TO_TEXTURE_TARGETS_EXT,
				&value);
		if((value & GLX_TEXTURE_2D_BIT_EXT) == 0)
			continue;


		/* Get the info on RGBA bind-to-texture support. */
		glXGetFBConfigAttrib(dpy, fbconfigs[i], rgba?GLX_BIND_TO_TEXTURE_RGBA_EXT:GLX_BIND_TO_TEXTURE_RGB_EXT,
				&value);
		if(value == GL_FALSE)
				  continue;
		break;
	}

	printf("Number of FBConfigs: %d\n", nfbconfigs);

	if(i == nfbconfigs)
		return NULL;

	return &fbconfigs[i];
};

int main(int argc, char **argv) {
	XvPortID a_port;
	XWindowAttributes xwattr;
	XSetWindowAttributes wattr;
	GLXFBConfig *fbconfig;
	for(int i = 1; i < argc; i++) {
		if(strncmp("-noshm", argv[i], 7) == 0)
			use_shared = false;
		if(strncmp("-notfp", argv[i], 7) == 0)
			use_tfp = false;
		if(strncmp("-xv", argv[i], 7) == 0)
			use_xv = true;
		if(strncmp("-nogl", argv[i], 7) == 0)
			want_opengl = false;
	}

	if(use_xv && !use_tfp) {
		printf("You cannot mix -notfp and -xv\n");
		return 1;
	}

	if(want_opengl) {
		/*
		SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
		SDL_GL_SetAttribute(SDL_GL_RED_SIZE, 8);
		SDL_GL_SetAttribute(SDL_GL_GREEN_SIZE, 8);
		SDL_GL_SetAttribute(SDL_GL_BLUE_SIZE, 8);
		SDL_GL_SetAttribute(SDL_GL_ALPHA_SIZE, 8);
		SDL_GL_SetAttribute(SDL_GL_DEPTH_SIZE, 24);
		*/
	}

	/*
	SetRes(1280, 800, want_opengl);

	SDL_VERSION(&wminfo.version);
	SDL_GetWMInfo(&wminfo);

	Display *dpy = wminfo.info.x11.gfxdisplay;
	Window wnd = wminfo.info.x11.window;
	*/
	GLXWindow glxw;
	GLXContext glxc;
	Display *dpy = XOpenDisplay(NULL);
	wattr.event_mask = ExposureMask|KeyPressMask|StructureNotifyMask;
	Window wnd = XCreateWindow(dpy, DefaultRootWindow(dpy), 0, 0, 1280, 800,
			1, 24, InputOutput, DefaultVisual(dpy, 0), 0, NULL);


	printf("Pointer to Display: %p\n", dpy);

	XMapWindow(dpy, wnd);

	if(want_opengl) {
		const int glx_attrib_list[] = {
			GLX_DOUBLEBUFFER, True,
			None, None };
		Bool r;
		fbconfig = get_suitable_fbconfig(dpy, wnd, !use_xv);
		printf("FBConfig pointer: %p\n", fbconfig);

		glxc = glXCreateNewContext(dpy, *fbconfig, GLX_RGBA_TYPE, NULL, True);
		printf("glxc: %p\n", glxc);
		glXCreateWindow(dpy, *fbconfig, wnd, glx_attrib_list);
		r = glXMakeContextCurrent(dpy, wnd, wnd, glxc);
		if(r == False) {
			printf("GLError from makecontextcurrent: %d\n", glGetError());
		}
		glViewport(0, 0, 1280, 800);
		glMatrixMode(GL_PROJECTION);
		glLoadIdentity();
		glOrtho(0., 1280., 800., 0., 1., 1024.);

		glMatrixMode(GL_MODELVIEW);
		glLoadIdentity();
		glTranslatef(0.f, 0.f, -10.f);
		glScalef(1280.f, 800.f, 1.f);
	}

	/* Query Xv extension to verify we have an Xv adaptor. If not, force Xv off. */
	unsigned nadaptors;
	int nimgfmts;
	XvAdaptorInfo *adaptor_infos;
	XvImageFormatValues *f_values, *desired = NULL;
	XvQueryAdaptors(dpy, wnd, &nadaptors, &adaptor_infos);
	printf("Number of Xv Adaptors: %d\n", nadaptors);

	if(nadaptors < 1) {
		printf("Cannot use Xv!\n");
		use_xv = false;
	} else {
		dump_xv_adaptor_infos(adaptor_infos, nadaptors);
		a_port = adaptor_infos->base_id; /* XXX: Just bind to the first adaptor port */
	}

	XvFreeAdaptorInfo(adaptor_infos);
	adaptor_infos = NULL;

	/* Next, gather the supported image formats. */
	f_values = XvListImageFormats(dpy, a_port, &nimgfmts);
	if(nimgfmts < 1) {
		printf("Cannot use Xv - No image formats!\n");
		use_xv = false;
	} else {
		dump_xv_imgfmt_infos(f_values, nimgfmts);
		for(int i = 0; i < nimgfmts; i++)
			/* IYUV and I420 are identical. */
			if((f_values[i].id == FOURCC_I420) || (f_values[i].id == FOURCC_IYUV)) {
				printf("Found I420 at %d\n", i);
				desired = &f_values[i];
				break;
			}
	}

	if(desired == NULL) {
		printf("Cannot use Xv - Could not find I420 format!\n");
		use_xv = false;
	}



   bzero(&x_shm_returns, sizeof(x_shm_returns));
   XShmQueryVersion(dpy, &x_shm_returns.major,
		&x_shm_returns.minor, &x_shm_returns.pixmaps);
	printf("X SHM Info: %d.%d pixmaps? %s\n", x_shm_returns.major, x_shm_returns.minor,
		x_shm_returns.pixmaps == True ? "Y":"N");

	if(want_opengl) {
		glEnable(GL_BLEND);
		glEnable(GL_DEPTH_TEST);

		glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
		glLoadIdentity();
		glTranslatef(0.f, 0.f, -1.f);
	}

	/* Use a 1920x1080 surface. */
	XShmSegmentInfo shm_seg_info;
	XImage *s_image = NULL; /* For normal texture_from_pixmap */
	XvImage *s_xvimage = NULL; /* For Xv tests */
#define IMG_WIDTH    1920
#define IMG_HEIGHT   1080

	bzero(&shm_seg_info, sizeof(shm_seg_info));

	/* Allocate the SHM region. */
	if(use_xv) {
		/* For YUV, allocate enough space for an 8-bit Y plane, and 2x2 subsampled U and V planes */
		shm_seg_info.shmid = shmget(IPC_PRIVATE, 3 * (IMG_WIDTH*IMG_HEIGHT >> 1), IPC_CREAT|0777);
	} else {
		/* For RGBA, allocate a full RGBA surface. */
		shm_seg_info.shmid = shmget(IPC_PRIVATE, 4*IMG_WIDTH*IMG_HEIGHT, IPC_CREAT|0777);
	}
	shm_seg_info.shmaddr = (char*)shmat(shm_seg_info.shmid, NULL, 0);
	shm_seg_info.readOnly = False;
	printf("SHM Address Pointer: %p\n", shm_seg_info.shmaddr);
	
	/* Build a SDL_Surface that targets the SHM region. */
	/*SDL_Surface *sdl_s = NULL;
	if(!use_xv)
		sdl_s = SDL_CreateRGBSurfaceFrom(shm_seg_info.shmaddr, IMG_WIDTH, IMG_HEIGHT,
				32, IMG_WIDTH*4, 0x00ff000000, 0x0000ff00, 0x000000ff, 0xff000000);
	*/

	char *p = NULL;

	p = shm_seg_info.shmaddr;
	GC xgc;

	Pixmap s_pixmap;

	XGetWindowAttributes(dpy, wnd, &xwattr);
	printf("Window depth: %d\n", xwattr.depth);
	xwattr.visual = DefaultVisual(dpy, 0);
	if(use_tfp) {
		/* Create an XImage object that targets the SHM region. X11 uses this as the source for Pixmap writes. */
		if(use_shared) {
			if(!use_xv) {
				s_image = XShmCreateImage(dpy, want_opengl ? glXGetVisualFromFBConfig(dpy, *fbconfig)->visual : xwattr.visual,
						want_opengl ? 32:24, ZPixmap, NULL, &shm_seg_info, IMG_WIDTH, IMG_HEIGHT);
				s_image->data = shm_seg_info.shmaddr;
				printf("XImage Pointer: %p, depth: %d\n", s_image, s_image->depth);
			} else {
				int r = XvGrabPort(dpy, a_port, CurrentTime);
				s_xvimage = XvShmCreateImage(dpy, a_port, desired->id, NULL, IMG_WIDTH, IMG_HEIGHT,
						&shm_seg_info);
				s_xvimage->data = shm_seg_info.shmaddr;
				printf("XvImage Pointer: %p (data: %p), Grab return: %d\n", s_xvimage, s_xvimage->data, r);
			}
		} else {
			if(!use_xv) {
				s_image = XCreateImage(dpy, want_opengl ? glXGetVisualFromFBConfig(dpy, *fbconfig)->visual : xwattr.visual,
						want_opengl ? 32 : 24, ZPixmap, 0, p, IMG_WIDTH, IMG_HEIGHT, 32, 0);
				printf("XImage Pointer: %p, depth: %d\n", s_image, s_image->depth);
			} else {
				int r = XvGrabPort(dpy, a_port, CurrentTime);
				s_xvimage = XvCreateImage(dpy, a_port, desired->id, p, IMG_WIDTH, IMG_HEIGHT);
				printf("XvImage Pointer: %p (data: %p), Grab return: %d\n", s_xvimage, s_xvimage->data, r);
			}
		}

		/* Allocate an off-screen Pixmap of 1920x1080 in the X11 server. */
		s_pixmap = XCreatePixmap(dpy, wnd, IMG_WIDTH, IMG_HEIGHT, (use_xv && want_opengl)?24:32);
		printf("Pixmap Pointer: %p\n", s_pixmap);

		/* Create a NO-OP GC. */
		xgc = XCreateGC(dpy, (want_opengl) ? s_pixmap:wnd, 0, NULL);

		if(want_opengl) {
			/* Get the GLX function pointers for GLX_EXT_texture_from_pixmap extension. */
			glXBindTexImageEXT = (void (*)(Display*, GLXDrawable, int, const int*))glXGetProcAddress((GLubyte*)"glXBindTexImageEXT");
			glXReleaseTexImageEXT = (void (*)(Display*, GLXDrawable, int))glXGetProcAddress((GLubyte*)"glXReleaseTexImageEXT");
		}
	}

	/* Texture setup. */
	GLuint tex;
	GLXPixmap glxp;
	const int glx_pixmap_attrs[] = {
		GLX_TEXTURE_TARGET_EXT, GLX_TEXTURE_2D_EXT,
		GLX_TEXTURE_FORMAT_EXT, use_xv?GLX_TEXTURE_FORMAT_RGB_EXT:GLX_TEXTURE_FORMAT_RGBA_EXT,
		None };

	if(want_opengl) {
		glEnable(GL_TEXTURE_2D);
		glTexEnvi(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE);

		glGenTextures(1, &tex);

		/* Create a GLX Pixmap handle to reference the X11 Pixmap in video memory for OpenGL. */
		if(use_tfp) {
			glxp = glXCreatePixmap(dpy, *fbconfig, s_pixmap, glx_pixmap_attrs);
		}
		glBindTexture(GL_TEXTURE_2D, tex);
		glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
		glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	}

	if(use_tfp) {
		/* Bind the GLX Pixmap to a Texture, connecting this texture handle to the X11 Pixmap. */
		if(want_opengl)
			glXBindTexImageEXT(dpy, glxp, GLX_FRONT_LEFT_EXT, NULL);

		if(use_shared) {
			/* Tell the X11 server to attach to the SHM region. */
			XShmAttach(dpy, &shm_seg_info);

			/* Tell the shm region to clean up after close. */
			shmctl(shm_seg_info.shmid, IPC_RMID, NULL);
		}
	}

	float color = 0.2f;
	unsigned int clr = 0xff000000;
	struct timespec ts_start, ts_stop;
	for(int c = 0; c != ~0U; c++) {
		int r;
		double timer_value;

		if(want_opengl)
			glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

		clock_gettime(CLOCK_MONOTONIC, &ts_start);

		/* Transfer the data from the XImage to the Pixmap (the texture buffer) */
		 printf("Putting...");
		 if(use_tfp) {
			 if(!use_xv) {
				 if(use_shared) {
					 r = XShmPutImage(dpy, want_opengl ? s_pixmap : wnd, xgc, s_image, 0, 0, 0, 0, IMG_WIDTH, IMG_HEIGHT, True);
				 } else {
					 r = XPutImage(dpy, want_opengl ? s_pixmap : wnd, xgc, s_image, 0, 0, 0, 0, IMG_WIDTH, IMG_HEIGHT);
				 }
			 } else {
				 XvStopVideo(dpy, a_port, (want_opengl)?s_pixmap:wnd);
				 if(use_shared) {
					 r = XvShmPutImage(dpy, a_port, (want_opengl)?s_pixmap:wnd, xgc, s_xvimage, 0, 0, IMG_WIDTH, IMG_HEIGHT,
							 0, 0, IMG_WIDTH, IMG_HEIGHT, True);
				 } else {
					 r = XvPutImage(dpy, a_port, (want_opengl)?s_pixmap:wnd, xgc, s_xvimage, 0, 0, IMG_WIDTH, IMG_HEIGHT,
							 0, 0, IMG_WIDTH, IMG_HEIGHT);
				 }
			 }
			 printf("%d\n", r);
			 /* Force X11 calls to complete. */
			 //XFlush(dpy);
		 } else {
			 if(want_opengl)
				 glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, IMG_WIDTH, IMG_HEIGHT, 0, GL_BGRA, GL_UNSIGNED_INT_8_8_8_8_REV, p);
		 }
		 printf("Blitting...%d\n", r);

		 if(use_shared) {
			 XEvent ev;
			 while(XCheckTypedEvent(dpy, XShmGetEventBase(dpy), &ev) == False) {
				 usleep(1000);
			 }
			 printf("Got XSHM Event\n");
		 }

		//usleep(500000);
		/* Blit a rectangle to the screen containing the pixmap texture. */
		 if(want_opengl) {
			 glBegin(GL_QUADS);
			 glColor3f(1.f, 1.f, 1.f);
			 color += 0.02f;
			 glTexCoord2d(0.,0.);
			 glVertex2f(0.f, 0.f);
			 glTexCoord2d(1.,0.);
			 glVertex2f(0.f, 720.f);
			 glTexCoord2d(1.,1.);
			 glVertex2f(1280.f, 720.f);
			 glTexCoord2d(0.,1.);
			 glVertex2f(1280.f, 0.f);
			 glEnd();
		 }

		clock_gettime(CLOCK_MONOTONIC, &ts_stop);

		/* Swap GL buffers. */
		printf("Swapping...\n");
		if(want_opengl) {
			glFlush();
			glXSwapBuffers(dpy, wnd);
		} else {
			XSync(dpy, False);
		}
		//usleep(500000);
		
		timer_value = static_cast<double>(ts_stop.tv_sec) - static_cast<double>(ts_start.tv_sec) +
			(static_cast<double>(ts_stop.tv_nsec - ts_start.tv_nsec) / 1000000000.);
		printf("PutImage + Blit:\t%0.6f msec\n", timer_value * 1000.);

	   clock_gettime(CLOCK_MONOTONIC, &ts_start);
		/* Write a new pattern into the surface. */
		 printf("Painting...\n");
		if(use_xv) {
			unsigned char *y_plane = (unsigned char*)shm_seg_info.shmaddr;
			unsigned char *u_plane = &(y_plane[IMG_HEIGHT*IMG_WIDTH]);
			unsigned char *v_plane = &(u_plane[IMG_HEIGHT*IMG_WIDTH >> 2]);
			for(int i = 0; i < IMG_HEIGHT; i++) {
				for(int j = 0; j < IMG_WIDTH; j++) {
					y_plane[(i * IMG_WIDTH) + j] = 0xff;
					if(((j % 2) == 0) && ((i % 2) == 0)) {
						u_plane[((i >> 1) * (IMG_WIDTH >> 1)) + (j >> 1)] = (clr & 0xff);
						v_plane[((i >> 1) * (IMG_WIDTH >> 1)) + (j >> 1)] = (clr++ & 0xff);
					}
				};
			};
			clr++;
		} else {
			p = (char*)shm_seg_info.shmaddr;
			if(want_opengl) {
				for(int i = 0; i < IMG_HEIGHT; i++) {
					for(int j = 0; j < IMG_WIDTH; j++) {
						*(unsigned long*)(&p[i*IMG_WIDTH*4 + (j << 2)]) = 0xff000000 | (clr++ & 0xffffff);
					};
				};
			} else {
				for(int i = 0; i < IMG_HEIGHT; i++) {
					for(int j = 0; j < IMG_WIDTH; j++) {
						unsigned int pxl = 0xff000000 | clr++ & 0xffffff;
						memcpy(p, &pxl, 4);
						p += 4;
					};
				};
			};
		};
	  clock_gettime(CLOCK_MONOTONIC, &ts_stop);
		
		timer_value = static_cast<double>(ts_stop.tv_sec) - static_cast<double>(ts_start.tv_sec) +
			(static_cast<double>(ts_stop.tv_nsec - ts_start.tv_nsec) / 1000000000.);
		printf("Fill Img:\t\t%0.6f msec\n", timer_value * 1000.);
	}

	glBindTexture(GL_TEXTURE_2D, 0);
	if(use_tfp) {
		glXReleaseTexImageEXT(dpy, glxp, GLX_FRONT_LEFT_EXT);
		glXDestroyPixmap(dpy, glxp);

		XFreePixmap(dpy, s_pixmap);
		if(use_shared)
			XShmDetach(dpy, &shm_seg_info);
	}

	if(use_xv)
		XvUngrabPort(dpy, a_port, CurrentTime);

	if(!use_xv) {
		/*delete s;
		SDL_FreeSurface(sdl_s);*/
	}
	shmdt(shm_seg_info.shmaddr);

	if(want_opengl) {
		glXDestroyWindow(dpy, glxw);
		glXDestroyContext(dpy, glxc);
	}

	XDestroyWindow(dpy, wnd);
	XCloseDisplay(dpy);
	/* SDLmm::Quit(); */

	return 0;
};