#include <stdlib.h>
#include <dmedia/vl.h>
#include <gl/gl.h>
#include <unistd.h>

void CheckSuccessVal(long, char *fncname);
void CheckSuccessPtr(void *, char *fncname);
void SendData(int a, int b, int c, int d,
	      int xsize, int ysize, ulong *dataPtr);

#define ROBA_R 0
#define ROBA_G 0
#define ROBA_B 0

#define ROBB_R 255
#define ROBB_G 255
#define ROBB_B 255

main(char *argv[], int argc) {
  VLServer svr;
  VLPath path;
  VLNode src, drn;
  VLBuffer buff;
  VLInfoPtr info;
  VLControlValue val;
  VLDevList devlist;
  int xsize, ysize;
  int x, y;
  int c;
  long p;
  long i;
  ulong *dataPtr, *moded;
  FILE *fp;
  long win, win2;
  int robax = 0, robay = 0;
  int robbx = 255, robby = 255;
  int besta, bestb, score;
  ulong white[10000], black[10000];
  float top = 1., dif = 0.;

  CheckSuccessPtr(svr = vlOpenVideo(NULL), "vlOpenVideo");

  /* Get the list of devices the daemon supports */
  /*CheckSuccessVal(vlGetDeviceList(svr, &devlist) >= 0, "vlGetDeviceList");*/

  /* Set up a drain node in memory */
  drn = vlGetNode(svr, VL_DRN, VL_MEM, VL_ANY);

  /* Set up a video source node */
  src = vlGetNode(svr, VL_SRC, VL_VIDEO, VL_ANY);

  /* Create a path on the first available device */
  CheckSuccessPtr((void *) path = vlCreatePath(svr, VL_ANY, src, drn),
		  "vlCreatePath");

  /* Set up the hardware for and define the usage of the path */
  CheckSuccessVal(vlSetupPaths(svr, (VLPathList)& path, 1, VL_SHARE, VL_SHARE),
		  "vlSetupPaths");

  /* Set the packing to RGB */
  val.intVal = VL_PACKING_RGB_8;
  vlSetControl(svr, path, drn, VL_PACKING, &val);
  
  /* Get the video size */
  vlGetControl(svr, path, drn, VL_SIZE, &val);
  xsize = val.xyVal.x;
  ysize = val.xyVal.y;

  /* Set up and open a GL window to display the data */
  foreground();
  prefsize(xsize,ysize);
  win = winopen("Simplegrab Window");
  RGBmode();
  pixmode(PM_TTOB, 1);
  gconfig();
  
  CheckSuccessPtr(buff = vlCreateBuffer(svr, path, drn, 1), "vlCreateBuffer");

  CheckSuccessVal(vlRegisterBuffer(svr, path, drn, buff), "vlRegisterBuffer");

  CheckSuccessVal(vlBeginTransfer(svr, path, 0, NULL), "vlBeginTransfer");

  i = 200;
  while (i--) {
    /* Wait for a frame */
    do {
      info = vlGetNextValid(svr, buff);
    } while (!info);
    
    /* Get a pointer to the frame */
    CheckSuccessPtr(dataPtr = (ulong *) vlGetActiveRegion(svr, buff, info),
		    "vlGetActiveRegion");

    lrectwrite(0,0, xsize-1, ysize-1, dataPtr);;

    /* Finished with frame, unlock the buffer */
    vlPutFree(svr, buff);
  }

  while (1) {
    /* Wait for a frame */
    do {
      info = vlGetNextValid(svr, buff);
    } while (!info);
    
    /* Get a pointer to the frame */
    CheckSuccessPtr(dataPtr = (ulong *) vlGetActiveRegion(svr, buff, info),
		    "vlGetActiveRegion");

    moded = malloc(xsize * ysize * sizeof(ulong));
    for (i = 0; i < xsize*ysize; i++)
      moded[i] = 0;
    for (i = 0; i < xsize*ysize; i++) {
      y = i / xsize;
      x = (i % xsize - xsize/2) * (dif * ((float) y / (float) ysize) + top);
      if (!(x < -xsize/2 || x > xsize/2 || y < 0 || y > ysize))
	moded[y * xsize + x + xsize/2] = dataPtr[i];
    }

    lrectwrite(0,0, xsize-1, ysize-1, moded);

    free(moded);

    /* Finished with frame, unlock the buffer */
    vlPutFree(svr, buff);

    /* Form matrix */
    printf("Adjust? ");
    fflush(stdin);
    if (getchar() == 'n')
      break;
    printf("top: ");
    scanf("%g", &top);
    printf("bottom: ");
    scanf("%g", &dif);
    dif = dif - top;
  }

  while (1) {
    /* Wait for a frame */
    do {
      info = vlGetNextValid(svr, buff);
    } while (!info);

    /* Get a pointer to the frame */
    CheckSuccessPtr(dataPtr = (ulong *) vlGetActiveRegion(svr, buff, info),
		    "vlGetActiveRegion");

    /* Find two locks for the centers of two robots */
    /* This is done by color coding */
    besta = bestb = 10000;
    for (i = 5 * xsize + 5; i < ysize*xsize; i++) {
      p = dataPtr[i];
      y = i / xsize;
      x = (i % xsize - xsize/2) * (dif * ((float) y / (float) ysize) + top);
      if (x < -xsize/2 || x > xsize/2 || y < 0 || y > ysize)
	continue;

      /*score = abs((0x0000FF & p) - ROBA_R)
	+ abs((0x00FF00 & p) / 0x000100 - ROBA_G)
	+ abs((0xFF0000 & p) / 0x010000 - ROBA_B);*/
      score = (0x0000FF & p) + ((0x00FF00 & p) >> 8) + ((0xFF0000 & p) >> 16);
      if (score < besta) {
	besta = score;
	robax = x + xsize/2;
	robay = y;
      }

      score = 0x1FF - (0x0000FF & p) - ((0x00FF00 & p) >> 8)
	- ((0xFF0000 & p) >> 16);
      if (score < bestb) {
	bestb = score;
	robbx = x + xsize/2;
	robby = y;
      }
    }

    printf("(%d, %d), (%d, %d)\n", robax, robay, robbx, robby);

    SendData(1, 1, 1, 1, xsize, ysize, dataPtr);
    SendData(robax & 0x1, robax & 0x2, robax & 0x4, robax & 0x8,
	     xsize, ysize, dataPtr);
    SendData(robax & 0x10, robax & 0x20, robax & 0x40, robax & 0x80,
	     xsize, ysize, dataPtr);
    SendData(robay & 0x1, robay & 0x2, robay & 0x4, robay & 0x8,
	     xsize, ysize, dataPtr);
    SendData(robay & 0x10, robay & 0x20, robay & 0x40, robay & 0x80,
	     xsize, ysize, dataPtr);
    SendData(robby & 0x1, robby & 0x2, robby & 0x4, robby & 0x8,
	     xsize, ysize, dataPtr);
    SendData(robby & 0x10, robby & 0x20, robby & 0x40, robby & 0x80,
	     xsize, ysize, dataPtr);
    SendData(robby & 0x1, robby & 0x2, robby & 0x4, robby & 0x8,
	     xsize, ysize, dataPtr);
    SendData(robby & 0x10, robby & 0x20, robby & 0x40, robby & 0x80,
	     xsize, ysize, dataPtr);
    SendData(0, 0, 0, 0, xsize, ysize, dataPtr);

    /* Finished with frame, unlock the buffer */
    vlPutFree(svr, buff);
  }

  /* End the data transfer */
  vlEndTransfer(svr, path);

  /* Wait until the user presses a key */
  printf("Press <Enter> to exit: \n");
  fflush(stdin);
  c = getc(stdin);
  c = getc(stdin);
  c = getc(stdin);
            
  /* Cleanup before exiting */
  vlDeregisterBuffer(svr, path, drn, buff);
  vlDestroyBuffer(svr, buff);
  vlDestroyPath(svr, path);
  vlCloseVideo(svr);
}

void CheckSuccessVal(long res, char *fncname) {
  if (res) {
    vlPerror(fncname);
    exit(1);
  }
}

void CheckSuccessPtr(void *ptr, char *fncname) {
  if (!ptr) {
    vlPerror(fncname);
    exit(1);
  }
}

void SendData(int a, int b, int c, int d,
	      int xsize, int ysize, ulong *dataPtr) {

  long i;
  long x = xsize / 4;

  /*  for (i = 0; i < 30000; i++) {
    if ((i % xsize) / x == 0)
      dataPtr[i] = a ? 0xFFFFFF : 0;
    if ((i % xsize) / x == 1)
      dataPtr[i] = b ? 0xFFFFFF : 0;
    if ((i % xsize) / x == 2)
      dataPtr[i] = c ? 0xFFFFFF : 0;
    if ((i % xsize) / x == 3)
      dataPtr[i] = d ? 0xFFFFFF : 0;
      }*/
  
  for (i = 0; i < 30000; i++)
    dataPtr[i] = a ? 0xFFFFFF : 0;

  /* Write the data to the screen */
  lrectwrite(0,0, xsize-1, ysize-1, dataPtr);

  sleep(.5);

  for (i = 0; i < 30000; i++)
    dataPtr[i] = b ? 0xFFFFFF : 0;

  /* Write the data to the screen */
  lrectwrite(0,0, xsize-1, ysize-1, dataPtr);

  sleep(.5);

  for (i = 0; i < 30000; i++)
    dataPtr[i] = c ? 0xFFFFFF : 0;

  /* Write the data to the screen */
  lrectwrite(0,0, xsize-1, ysize-1, dataPtr);

  sleep(.5);

  for (i = 0; i < 30000; i++)
    dataPtr[i] = d ? 0xFFFFFF : 0;

  /* Write the data to the screen */
  lrectwrite(0,0, xsize-1, ysize-1, dataPtr);

  sleep(.5);
}