#include <X11/X.h>
#include <X11/Xlib.h>
#include <X11/extensions/XShm.h>
#include <X11/extensions/Xvlib.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define DEBUG 0

#define FOURCC_YV12 0x32315659	/* ie. {'Y','V','1','2'} */

#define WIDTH  704
#define HEIGHT 480
#define LUMA_SIZE 	(WIDTH * HEIGHT)
#define CHROMA_SIZE	((WIDTH >> 1) * (HEIGHT >> 1))

#define DES 1
#define SRC 2

typedef struct clk_image
{
  XvImage *image;
  unsigned char *Ydata;
  unsigned char *Udata;
  unsigned char *Vdata;
  unsigned char *Yptr;
  unsigned char *Uptr;
  unsigned char *Vptr;
//      XShmSegmentInfo shminfo
} clk_image;

Display *dpy;
Window win;
GC context;
int portid = -1;

clk_image image;
int ckey_val, ckey_type;

static void
FreeData (clk_image * image)
{
  if (image->Ydata)
    free (image->Ydata);
  if (image->Udata)
    free (image->Udata);
  if (image->Vdata)
    free (image->Vdata);
}

static void
GetYUVFromRGB (int R, int G, int B, int *Y, int *U, int *V)
{
  *Y = ((66 * R + 129 * G + 25 * B + 128) >> 8) + 16;
  *U = ((-38 * R - 74 * G + 112 * B + 128) >> 8) + 128;
  *V = ((112 * R - 94 * G - 18 * B + 128) >> 8) + 128;
}

static void
GetRGBFromYUV (int Y, int U, int V)
{
  int R, G, B;
  int C, D, E;

  C = Y - 16;
  D = U - 128;
  E = V - 128;
  R = (298 * C + 409 * E + 128) >> 8;
  G = (298 * C - 100 * D - 208 * E + 128) >> 8;
  B = (298 * C + 516 * D + 128) >> 8;
  printf ("RGB is: %d, %d, %d\n", (unsigned char) R, (unsigned char) G,
	  (unsigned char) B);
}

static void
ParseCKeyToRGB (int *R, int *G, int *B)
{
  *R = (0xff0000 & ckey_val) >> 16;
  *G = (0x00ff00 & ckey_val) >> 8;
  *B = 0x0000ff & ckey_val;
}

static void
RevertRGB (int *R, int *G, int *B)
{
  *R = ~*R;
  *G = ~*G;
  *B = ~*B;
}

static void
SetYUVData (clk_image * image)
{
  int Y, U, V;
  int R, G, B;
  int i, j;

  ParseCKeyToRGB (&R, &G, &B);

  if (DEBUG)
    printf ("des: %d, %d, %d\n", Y, U, V);

  GetYUVFromRGB (R, G, B, &Y, &U, &V);
  if (DEBUG)
    printf ("RGB is: %d, %d, %d\n", (unsigned char) R, (unsigned char) G,
	    (unsigned char) B);
  if (DEBUG)
    printf ("des: %d, %d, %d\n", Y, U, V);
  //GetRGBFromYUV(Y, U, V);       
  for (i = 0; i < (WIDTH * HEIGHT) / 2; i++)
    {
      image->Ydata[i] = (unsigned char) Y;
    }
  for (i = 0; i < ((WIDTH >> 1) * (HEIGHT >> 1)) / 2; i++)
    {
      image->Udata[i] = (unsigned char) U;
      image->Vdata[i] = (unsigned char) V;
    }

  RevertRGB (&R, &G, &B);
  GetYUVFromRGB (R, G, B, &Y, &U, &V);
  if (DEBUG)
    printf ("RGB is: %d, %d, %d\n", (unsigned char) R, (unsigned char) G,
	    (unsigned char) B);
  //GetRGBFromYUV(Y, U, V);       

  if (DEBUG)
    printf ("des: %d, %d, %d\n", Y, U, V);
  j = (WIDTH * HEIGHT) / 2;
  for (i = 0; i < (WIDTH * HEIGHT) / 2; i++)
    {
      image->Ydata[i + j] = (unsigned char) Y;
    }
  j = ((WIDTH >> 1) * (HEIGHT >> 1)) / 2;
  for (i = 0; i < ((WIDTH >> 1) * (HEIGHT >> 1)) / 2; i++)
    {
      image->Udata[i + j] = (unsigned char) U;
      image->Vdata[i + j] = (unsigned char) V;
    }
  if (DEBUG)
    printf ("create des date\n");
}

static int
CreateYUVData (clk_image * image)
{
  if (DEBUG)
    printf ("CreateYUVData\n");

  image->Ydata = (unsigned char *) malloc (WIDTH * HEIGHT);
  image->Udata = (unsigned char *) malloc ((WIDTH >> 1) * (HEIGHT >> 1));
  image->Vdata = (unsigned char *) malloc ((WIDTH >> 1) * (HEIGHT >> 1));
  if (!image->Ydata || !image->Udata || !image->Vdata)
    {
      FreeData (image);
      fprintf (stderr, "Failed to allocate memory\n");
      return 0;
    }

  if (DEBUG)
    printf ("SetYUVData\n");

  SetYUVData (image);
  return 1;
}

static int
SetupSurfaces (clk_image * image)
{
  if (DEBUG)
    printf ("SetupSurfaces\n");

  image->image = XvCreateImage (dpy, portid, FOURCC_YV12, 0, WIDTH, HEIGHT);

  if (!image->image)
    {
      printf ("failed to create XvImage\n");
      return 0;
    }

  /* setup two XvImages with the same shm segment */
   /** image->shminfo.shmid = shmget (IPC_PRIVATE, image->image->data_size * 2, IPC_CREAT|0777);
    image->shminfo.shmaddr = image->image->data = shmat (image->shminfo.shmid, 0, 0);
    shmctl (image->shminfo.shmid, IPC_RMID, 0);
    image->shminfo.readOnly = False;
    if(!XShmAttach (dpy, &(image->shminfo))) {
	fprintf(stderr, "XShmAttach failed!\n");
	shmdt (image->shminfo.shmaddr);
	XFree(image->image);
	image->image = NULL;
	return 0;
    }
*/
  image->image->data =
    (unsigned char *) malloc (sizeof (unsigned char) * WIDTH * HEIGHT * 2);
  image->Yptr = image->image->data + image->image->offsets[0];
  image->Vptr = image->image->data + image->image->offsets[1];
  image->Uptr = image->image->data + image->image->offsets[2];

  return 1;
}

static void
CopyData (clk_image * image)
{
  memcpy (image->Yptr, image->Ydata, LUMA_SIZE);
  memcpy (image->Uptr, image->Udata, CHROMA_SIZE);
  memcpy (image->Vptr, image->Vdata, CHROMA_SIZE);
}

static int
CreateImage (clk_image * image)
{
  if (DEBUG)
    printf ("create image\n");

  if (!CreateYUVData (image) || !SetupSurfaces (image))
    {
      FreeData (image);
      XCloseDisplay (dpy);
      return 0;
    }

  //SetDrawBuffer();
  CopyData (image);
  return 1;
}

static int
GetPortId (Display * dpy)
{
  unsigned int i, j, k, numAdapt, portNum;
  int numImages;
  XvImageFormatValues *formats;
  XvAdaptorInfo *info;
  XvAttribute *attrs;
  int nattr;

  portNum = -1;

  if (Success !=
      XvQueryAdaptors (dpy, DefaultRootWindow (dpy), &numAdapt, &info))
    return -1;

  for (i = 0; i < numAdapt; i++)
    {
      if (info[i].type & XvImageMask)
	{
	  /* Adaptor has XvImage support */
	  formats = XvListImageFormats (dpy, info[i].base_id, &numImages);

	  for (j = 0; j < numImages; j++)
	    {
	      if (formats[j].id == FOURCC_YV12)
		{
		  /* It supports our format */
		  for (k = 0; k < info[i].num_ports; k++)
		    {
		      /* try to grab a port */
		      if (Success == XvGrabPort (dpy, info[i].base_id + k,
						 CurrentTime))
			{
			  portNum = info[i].base_id + k;
			  break;
			}
		    }
		}
	      if (portNum == -1)
		continue;
	      attrs = XvQueryPortAttributes (dpy, info[i].base_id, &nattr);
	      if (attrs && nattr)
		{
		  for (k = 0; k < nattr; k++)
		    {
		      if (!strcmp (attrs[k].name, "XV_COLORKEY") &&
			  ckey_type == DES)
			break;
		      else if (!strcmp (attrs[k].name, "XV_SCOLORKEY") &&
			       ckey_type == SRC)
			break;
		    }
		  if (k == nattr)
		    portNum = -1;
		}
	      if (portNum != -1)
		break;
	    }
	  XFree (formats);
	}
      if (portNum != -1)
	break;
    }

  XvFreeAdaptorInfo (info);

  return portNum;
}

static int
SetupXvStuff (void)
{
  unsigned int ver, rev, reqBase, eventBase, errorBase;

  if (Success != XvQueryExtension (dpy,
				   &ver, &rev,
				   &reqBase, &eventBase, &errorBase))
    {
      fprintf (stderr, "No Xv extension\n");
      return 0;
    }
  else
    fprintf (stderr, "Using Xv extension %i.%i\n", ver, rev);


  portid = GetPortId (dpy);
  if (portid == -1)
    {
      fprintf (stderr, "Couldn't find a free Xv port\n");
      return 0;
    }

  return 1;
}

static void
Draw (int width, int height)
{
  //XFlush(dpy);
  //get_pixel(10, 10);

  if (DEBUG)
    printf ("output image \n");
  //return;       
  printf ("pord id: %d\n", portid);
  Atom attr_atom;
  int cur_val;

  //attr_name="XV_COLORKEY";
  if (ckey_type & DES)
    attr_atom = XInternAtom (dpy, "XV_COLORKEY", False);
  else
    attr_atom = XInternAtom (dpy, "XV_SCOLORKEY", False);

  //change attribute value
  if (XvSetPortAttribute (dpy, portid, attr_atom, ckey_val) != Success)
    {
      printf ("xv color key: Couldn't set attribute\n");
      exit (-1);
    }
  //  XFlush(dpy);
  if (XvGetPortAttribute (dpy, portid, attr_atom, &cur_val) != Success)
    {
      fprintf (stderr, "Couldn't get attribute\n");
      exit (-1);
    }
  else
    {
      printf ("cur color-key is: %d\n", cur_val);
    }
  if (DEBUG)
    printf ("output image here\n");
  XvPutImage (dpy, portid, win, context,
	      image.image, 0, 0, WIDTH, HEIGHT, 0, 0, width, height);
  XFlush(dpy);
  //return;
  XSetForeground (dpy, context, ckey_val);
  XFillRectangle (dpy, win, context, 0, 0, width / 2, height);
  XSetForeground (dpy, context, ~ckey_val);
  XFillRectangle (dpy, win, context, width / 2, 0, width / 2, height);


  XFlush (dpy);
}

static void
DeleteShmSurfaces (clk_image * image)
{
//    XShmDetach (dpy, &(image->shminfo)); 
//    shmdt (image->shminfo.shmaddr);
  if (image->image)
    XFree (image->image);
}

int
main (int argv, char **argc)
{
  int width, height;
  XEvent ev;

// parse arguments
  if (argv != 3)
    {
      printf ("Usage: %s [-s|-d] colorkey_val\n", argc[0]);
      exit (-1);
    }
  else
    {
      if (!strcmp ("-s", argc[1]))
	ckey_type = SRC;
      else if (!strcmp ("-d", argc[1]))
	ckey_type = DES;
      else
	{
	  printf ("Usage: %s [-s|-d] colorkey_val\n", argc[0]);
	  exit (-1);
	}
      ckey_val = atoi (argc[2]);
    }

  printf ("starting up video testapp...\n\n");

  dpy = XOpenDisplay (NULL);
  if (dpy == NULL)
    {
      printf ("Cannot open Display.\n");
      exit (-1);
    }

  // get xvinfo, read image data, create xv image
  if (!SetupXvStuff ())
    return -1;

  if (!CreateImage (&image))
    return -1;

  width = WIDTH;		/* start out at the source size */
  height = HEIGHT;

  win = XCreateSimpleWindow (dpy, DefaultRootWindow (dpy),
			     0, 0, width, height, 0, 0, 0);
  XSetWindowBackgroundPixmap (dpy, win, None);
  XMapWindow (dpy, win);
  XSelectInput (dpy, win,
		KeyPressMask | ButtonPressMask | Button1MotionMask |
		ConfigureNotify);
  context = XCreateGC (dpy, win, 0, 0);
  XSync (dpy, False);
  Draw (width, height);
  while (XNextEvent (dpy, &ev) == 0)
    {
      if (ev.type == ButtonPress)
	break;
    }

  XDestroyWindow (dpy, win);
  XFreeGC (dpy, context);
  FreeData (&image);
  DeleteShmSurfaces (&image);
  XCloseDisplay (dpy);
  return 1;
}