/*
 * This program captures images from a IEEE 1394 (FireWire) digital
 * camera conforming "IIDC 1394-based Digital Camera Specification"
 * and outputs images to stdout in the ffmpeg "rawvideo" format.
 * This program allows you to use a FireWire digital camera with
 * ffmpeg. 
 *
 * FireWire camcorders do not conform to that specification, and they
 * are already supported by "-dv1394" ffmpeg option.
 *
 * Installation:
 * You have to install libdc1394 (http://sf.net/projects/libdc1394)
 * and libraw1394 (http://www.linux1394.org).
 * To compile, run
 * gcc -O3 thisfile.c -ldc1394_control -lraw1394
 *
 * Usage: Run this program without options.
 */

/*
 * This program is based on grab_gray_image.c in libdc1394 0.9.0-2,
 * whose copyright notice is as follows.
 * This program requires libdc1394 0.9.1.
 */

/**************************************************************************
**       Title: grab one gray image using libdc1394
**    $RCSfile: grab_gray_image.c,v $
**   $Revision: 1.3 $$Name:  $
**       $Date: 2001/10/16 09:14:14 $
**   Copyright: LGPL $Author: ronneber $
** Description:
**
**    Get one gray image using libdc1394 and store it as portable gray map
**    (pgm). Based on 'samplegrab' from Chris Urmson 
**
**-------------------------------------------------------------------------
**
**  $Log: grab_gray_image.c,v $
**  Revision 1.3  2001/10/16 09:14:14  ronneber
**  - added more meaningful error message, when no raw1394 handle could be get
**  - does not exit anymore, when camera has no trigger
**
**  Revision 1.2  2001/09/14 08:10:41  ronneber
**  - some cosmetic changes
**
**  Revision 1.1  2001/07/24 13:50:59  ronneber
**  - simple test programs to demonstrate the use of libdc1394 (based
**    on 'samplegrab' of Chris Urmson
**
**
**************************************************************************/


#include <stdio.h>
#include <libraw1394/raw1394.h>
#include <libdc1394/dc1394_control.h>
#include <stdlib.h>
#include <unistd.h>
#include <signal.h>
#include <string.h>

#define PMIN(x,y) ((x) > (y) ? (y) : (x))
#define BLACK_Y 0
#define BLACK_U 128
#define BLACK_V 128
#define FALSE 0
#define TRUE 1

inline int uyvy422_yuv420p(const unsigned char *srcFrameBuffer,
		unsigned char *dstFrameBuffer)
{
  const int srcFrameWidth = 320,
    srcFrameHeight = 240,
    dstFrameWidth = 320,
    dstFrameHeight = 240;
  unsigned int row,column;
  unsigned char *y = dstFrameBuffer;  /*Initialise y,u,v here, to stop compiler warnings.*/
  unsigned char *u = dstFrameBuffer + dstFrameWidth*dstFrameHeight;
  unsigned char *v = dstFrameBuffer + dstFrameWidth*(dstFrameHeight + dstFrameHeight/4);
  const unsigned char *src = srcFrameBuffer;

  if (srcFrameBuffer == dstFrameBuffer)
    return FALSE;

  for(row=0; row < PMIN(srcFrameHeight, dstFrameHeight); row+=2) {
    y = dstFrameBuffer + dstFrameWidth*row;
    u = dstFrameBuffer + dstFrameWidth*dstFrameHeight + dstFrameWidth*row/4;
    v = dstFrameBuffer + dstFrameWidth*(dstFrameHeight + dstFrameHeight/4) + dstFrameWidth*row/4;
    src = srcFrameBuffer + row*srcFrameWidth*2;
    for(column=0; column < PMIN(srcFrameWidth, dstFrameWidth); column+=2) {
      *(u++) = (unsigned char)(((int)src[0] + src[srcFrameWidth*2])/2);
      *(y++) = src[1];
      *(v++) = (unsigned char)(((int)src[2] + src[2+srcFrameWidth*2])/2);
      *(y++) = src[3];
      src += 4;
    }
    for(column = PMIN(srcFrameWidth, dstFrameWidth);
	column < dstFrameWidth; column+=2) {
      *(u++) = BLACK_U;
      *(y++) = BLACK_Y;
      *(v++) = BLACK_V;
      *(y++) = BLACK_Y;
    }
    y = dstFrameBuffer + dstFrameWidth*(row+1);
    src = srcFrameBuffer + (row+1)*srcFrameWidth*2;
    for(column=0; column < PMIN(srcFrameWidth,dstFrameWidth); column+=2) {
      src++;
      *(y++) = *(src++);
      src++;
      *(y++) = *(src++);
    }
    for(column = PMIN(srcFrameWidth, dstFrameWidth);
	column < dstFrameWidth; column+=2) {
      *(y++) = BLACK_Y;
      *(y++) = BLACK_Y;
    }
  }
  for(row = PMIN(srcFrameHeight, dstFrameHeight);
      row < dstFrameHeight; row+=2) {
    for(column = 0; column < dstFrameWidth; column+=2) {
      *(u++) = BLACK_U;
      *(y++) = BLACK_Y;
      *(v++) = BLACK_V;
      *(y++) = BLACK_Y;
    }
    for(column = 0; column < dstFrameWidth; column+=2) {
      *(y++) = BLACK_Y;
      *(y++) = BLACK_Y;
    }
  }
  return TRUE;
}

inline int uyv444_yuv420p(const unsigned char *srcFrameBuffer,
		unsigned char *dstFrameBuffer)
{
  const int srcFrameWidth = 160,
    srcFrameHeight = 120,
    dstFrameWidth = 160,
    dstFrameHeight = 120;
  unsigned int row,column;
  unsigned char *y = dstFrameBuffer;  /*Initialise y,u,v here, to stop compiler warnings.*/
  unsigned char *u = dstFrameBuffer + dstFrameWidth*dstFrameHeight;
  unsigned char *v = dstFrameBuffer + dstFrameWidth*(dstFrameHeight + dstFrameHeight/4);
  const unsigned char *src = srcFrameBuffer;

  if (srcFrameBuffer == dstFrameBuffer)
    return FALSE;

  for(row=0; row < PMIN(srcFrameHeight, dstFrameHeight); row+=2) {
    y = dstFrameBuffer + dstFrameWidth*row;
    u = dstFrameBuffer + dstFrameWidth*dstFrameHeight + dstFrameWidth*row/4;
    v = dstFrameBuffer + dstFrameWidth*(dstFrameHeight + dstFrameHeight/4) + dstFrameWidth*row/4;
    src = srcFrameBuffer + row*srcFrameWidth*3;
    for(column=0; column < PMIN(srcFrameWidth, dstFrameWidth); column+=2) {
      *(u++) = (unsigned char)(((unsigned int)src[0] + src[3] + src[srcFrameWidth*3] + src[3+srcFrameWidth*3])/4);
      *(y++) = src[1];
      *(v++) = (unsigned char)(((unsigned int)src[2] + src[5] + src[srcFrameWidth*3] +src[3+srcFrameWidth*3])/4);
      *(y++) = src[4];
      src += 6;
    }
    for(column = PMIN(srcFrameWidth, dstFrameWidth);
	column < dstFrameWidth; column+=2) {
      *(u++) = BLACK_U;
      *(y++) = BLACK_Y;
      *(v++) = BLACK_V;
      *(y++) = BLACK_Y;
    }
    y = dstFrameBuffer + dstFrameWidth*(row+1);
    src = srcFrameBuffer + (row+1)*srcFrameWidth*3;
    for(column=0; column < PMIN(srcFrameWidth, dstFrameWidth); column++) {
      src++;
      *(y++) = *(src++);
      src++;
    }
    for(column = PMIN(srcFrameWidth, dstFrameWidth);
	column < dstFrameWidth; column++)
      *(y++) = BLACK_Y;
  }
  for(row = PMIN(srcFrameHeight, dstFrameHeight);
      row<dstFrameHeight; row+=2) {
    for(column = 0; column < dstFrameWidth; column+=2) {
      *(u++) = BLACK_U;
      *(y++) = BLACK_Y;
      *(v++) = BLACK_V;
      *(y++) = BLACK_Y;
    }
    for(column = 0; column < dstFrameWidth; column+=2) {
      *(y++) = BLACK_Y;
      *(y++) = BLACK_Y;
    }
  }
  return TRUE;
}

inline int uyv411_yuv420p(const unsigned char *srcFrameBuffer,
		unsigned char *dstFrameBuffer)
{
  const int srcFrameWidth = 640,
    /* srcFrameHeight = 480, */
    dstFrameWidth = 640,
    dstFrameHeight = 480;
  int i,row,column;
  unsigned char *y = dstFrameBuffer;  /*Initialise y,u,v here, to stop compiler warnings.*/
  unsigned char *u = dstFrameBuffer + dstFrameWidth*dstFrameHeight;
  unsigned char *v = dstFrameBuffer + dstFrameWidth*(dstFrameHeight + dstFrameHeight/4);
  const unsigned char *src = srcFrameBuffer;

  if (srcFrameBuffer == dstFrameBuffer)
    return FALSE;

  for (i=0; i < (dstFrameWidth*dstFrameHeight)/4; i++) {
    src++;
    *y++ = *src++;
    *y++ = *src++;
    src++;
    *y++ = *src++;
    *y++ = *src++;
  }
  src = srcFrameBuffer;
  for (row=0; row < dstFrameHeight; row += 2) {
    src = srcFrameBuffer + (row*srcFrameWidth*3)/2;
    u = dstFrameBuffer + dstFrameWidth*dstFrameHeight + dstFrameWidth*row/4;
    v = dstFrameBuffer + dstFrameWidth*(dstFrameHeight + dstFrameHeight/4) + dstFrameWidth*row/4;
    for (column=0; column < dstFrameWidth; column += 4) {
      u[0] = u[1] = ((int)src[0] + src[(srcFrameWidth*3)/2])/2;
      v[0] = v[1] = ((int)src[3] + src[3+(srcFrameWidth*3)/2])/2;
      u += 2;
      v += 2;
      src += 6;
    }
  }
  return TRUE;
}

#define UNSET (-1)
static int frame_size = UNSET;
static int frame_rate = UNSET;
static char *device_name = NULL;
static int  capture_count = -1; /* forever */


static void process_size_opt(char *optarg)
{
  if (strcmp(optarg, "160x120") == 0)
    frame_size = MODE_160x120_YUV444; /* defined in dc1394_control.h */
  else if (strcmp(optarg, "320x240") == 0)
    frame_size = MODE_320x240_YUV422;
  else if (strcmp(optarg, "640x480") == 0)
    frame_size = MODE_640x480_YUV411;
  else {
    fprintf(stderr, "This program currently suppots frame sizes of"
	    " 160x120, 320x240, and 640x480.\n"
	    "Please specify one of them.\n"
	    "You have specified %s.\n", optarg);
    exit(0);
  }
}

static void process_rate_opt(char *optarg)
{
  float rate_f;
  sscanf(optarg, "%g", &rate_f);

  if (rate_f == 1.875)
    frame_rate = FRAMERATE_1_875;
  else if (rate_f == 3.75)
    frame_rate = FRAMERATE_3_75;
  else if (rate_f == 7.5)
    frame_rate = FRAMERATE_7_5;
  else if (rate_f == 15)
    frame_rate = FRAMERATE_15;
  else if (rate_f == 30)
    frame_rate = FRAMERATE_30;
  else if (rate_f == 60)
    frame_rate = FRAMERATE_60;
  else {
    fprintf(stderr, "This program supports framerates of"
	    " 1.875, 3.75, 7.5, 15, 30, 60\n"
	    "Please specify one of them.\n"
	    "You have specified %s.\n", optarg);
    exit(0);
  }
}

static void usage(void)
{
  fprintf(stderr, "This program accepts the following options.\n"
	  "You have to specify -d, -s, and -r.\n"
	  "-c <count>: <count> frames will be captured. -1 means infinity.\n"
	  "-d <device name>: Device name is usually /dev/raw1394 or /dev/video1394/0.\n"
	  "-s <frame size>: Frame size is one of 160x120, 320x240, or 640x480.\n"
	  "-r <frame rate>: Frame rate is one of 1.875, 3.75, 7.5, 15, 30, or 60.\n");
}

static void check_unset_opt(void)
{
  if (device_name == NULL) {
    fprintf(stderr, "The device name is not set. Please specify it with -d option.\n");
    usage();
    exit(1);
  }
  if (frame_size == UNSET) {
    fprintf(stderr, "The frame size is not set. Please specify it with -s option.\n");
    usage();
    exit(1);
  }
  if (frame_rate == UNSET) {
    fprintf(stderr, "The frame rate is not set. Please specify it with -r option.\n");
    usage();
    exit(1);
  }
}

static void process_options(int argc, char **argv)
{
  int optchar;

  while ((optchar = getopt(argc, argv, "s:r:d:c:")) != -1) {
    if (optchar == 'd')
      device_name = optarg;

    else if (optchar == 's')
      process_size_opt(optarg);
    else if (optchar == 'r')
      process_rate_opt(optarg);
    else if (optchar == 'c')
      sscanf(optarg, "%d", &capture_count);
    else {
      usage();
      exit(1);
    }
  }
  check_unset_opt();
}
      

int main(int argc, char *argv[]) 
{
  dc1394_cameracapture camera;
  int numNodes;
  int numCameras;
  raw1394handle_t handle;
  nodeid_t * camera_nodes;
  quadlet_t supported_framerates;
  unsigned char *buf;
  int i;
  int buf_size;

  process_options(argc, argv);

  /*-----------------------------------------------------------------------
   *  Open ohci and asign handle to it
   *-----------------------------------------------------------------------*/
  handle = dc1394_create_handle(0);
  if (handle==NULL)
  {
    fprintf( stderr, "Unable to aquire a raw1394 handle\n\n"
             "Please check \n"
	     "  - if the kernel modules `ieee1394',`raw1394' and `ohci1394' are loaded \n"
	     "  - if you have read/write access to /dev/raw1394\n\n");
    exit(1);
  }

  
  /*-----------------------------------------------------------------------
   *  get the camera nodes and describe them as we find them
   *-----------------------------------------------------------------------*/
  numNodes = raw1394_get_nodecount(handle);
  camera_nodes = dc1394_get_camera_nodes(handle,&numCameras,0);
  fflush(stdout);
  if (numCameras<1)
  {
    fprintf( stderr, "no cameras found :(\n");
    dc1394_destroy_handle(handle);
    exit(1);
  }
  fprintf(stderr, "working with the first camera on the bus\n");
  
  /*-----------------------------------------------------------------------
   *  to prevent the iso-transfer bug from raw1394 system, check if
   *  camera is highest node. For details see 
   *  http://linux1394.sourceforge.net/faq.html#DCbusmgmt
   *  and
   *  http://sourceforge.net/tracker/index.php?func=detail&aid=435107&group_id=8157&atid=108157
   *-----------------------------------------------------------------------*/
  for (i=0; i<numCameras; i++) {
    if( camera_nodes[i] == numNodes-1)
      {
	fprintf( stderr, "\n"
             "Sorry, your camera is the highest numbered node\n"
             "of the bus, and has therefore become the root node.\n"
             "The root node is responsible for maintaining \n"
             "the timing of isochronous transactions on the IEEE \n"
             "1394 bus.  However, if the root node is not cycle master \n"
             "capable (it doesn't have to be), then isochronous \n"
             "transactions will not work.  The host controller card is \n"
             "cycle master capable, however, most cameras are not.\n"
             "\n"
             "The quick solution is to add the parameter \n"
             "attempt_root=1 when loading the OHCI driver as a \n"
             "module.  So please do (as root):\n"
             "\n"
             "   rmmod ohci1394\n"
             "   insmod ohci1394 attempt_root=1\n"
             "\n"
             "for more information see the FAQ at \n"
             "http://linux1394.sourceforge.net/faq.html#DCbusmgmt\n"
             "\n");
	exit( 1);
      }
  }

  /*
   * Check fastest available framerate.
   */
  if (dc1394_query_supported_framerates(handle, camera_nodes[0],
					FORMAT_VGA_NONCOMPRESSED,
					frame_size,
					&supported_framerates) != DC1394_SUCCESS) {
    fprintf(stderr, "dc1394_query_supported_framerates() failed.");
    return 1;
  }

  if ((!(supported_framerates & (1U << (31-5))) &&
       (frame_rate == FRAMERATE_60)) ||
      (!(supported_framerates & (1U << (31-4))) &&
       (frame_rate == FRAMERATE_30)) ||
      (!(supported_framerates & (1U << (31-3))) &&
       (frame_rate == FRAMERATE_15)) ||
      (!(supported_framerates & (1U << (31-2))) &&
       (frame_rate == FRAMERATE_7_5)) ||
      (!(supported_framerates & (1U << (31-1))) &&
       (frame_rate == FRAMERATE_3_75)) ||
      (!(supported_framerates & (1U << (31-0))) &&
       (frame_rate == FRAMERATE_1_875))) {
    fprintf(stderr, "Your specified frame rate is not supported by your camera.\n");
    return 1;
  }

  /*-----------------------------------------------------------------------
   *  setup capture
   *-----------------------------------------------------------------------*/
  /* If an error occurs at the next line, maybe your libdc1394 is too old. */
  if (dc1394_dma_setup_capture(handle,camera_nodes[0],
			       0, /* channel */ 
			       FORMAT_VGA_NONCOMPRESSED,
			       frame_size,
			       SPEED_400,
			       frame_rate, 4, 0, 1, device_name,
			       &camera)!=DC1394_SUCCESS) 
  {
    fprintf( stderr,"unable to setup camera\n");
    dc1394_dma_release_camera(handle,&camera);
    dc1394_destroy_handle(handle);
    exit(1);
  }
  
  /*-----------------------------------------------------------------------
   *  have the camera start sending us data
   *-----------------------------------------------------------------------*/
  if (dc1394_start_iso_transmission(handle,camera.node)
      !=DC1394_SUCCESS) 
  {
    fprintf( stderr, "unable to start camera iso transmission\n");
    dc1394_dma_release_camera(handle,&camera);
    dc1394_destroy_handle(handle);
    exit(1);
  }

#if 0
  /* Discard first several dozen of frames, which are dirty. */
  for (i=0; i<60; i++) {
    if (dc1394_dma_single_capture(&camera)!=DC1394_SUCCESS) 
      {
	fprintf( stderr, "unable to capture a frame\n");
	dc1394_dma_release_camera(handle,&camera);
	dc1394_destroy_handle(handle);
	exit(1);
      }
    dc1394_dma_done_with_buffer(&camera);
  }
#endif

  buf_size = frame_size == MODE_160x120_YUV444 ? 160*120 :
    (frame_size == MODE_320x240_YUV422 ? 320*240 :
     (frame_size == MODE_640x480_YUV411 ? 640*480 : -3 /* ??? */));
  buf_size = buf_size * 3 / 2; /* YUV420 is 12 bits / pixel */
  buf = malloc(buf_size);
  if (buf == NULL) {
    fprintf(stderr, "malloc() failed.\n");
    exit(1);
  }

  signal(SIGPIPE, SIG_IGN);

  for (i=0; capture_count == -1 || i<capture_count; i++) {
    int num_written, current_written;

    if (dc1394_dma_single_capture(&camera)!=DC1394_SUCCESS) 
      {
	fprintf( stderr, "unable to capture a frame\n");
	dc1394_dma_release_camera(handle,&camera);
	dc1394_destroy_handle(handle);
	exit(1);
      }

    if (frame_size == MODE_160x120_YUV444)
      uyv444_yuv420p((const char *)camera.capture_buffer, buf);
    else if (frame_size == MODE_320x240_YUV422)
      uyvy422_yuv420p((const char *)camera.capture_buffer, buf);
    else if (frame_size == MODE_640x480_YUV411)
      uyv411_yuv420p((const char *)camera.capture_buffer, buf);
    else {
      fprintf(stderr, "Something wrong happend!\n");
      exit(1);
    }
    current_written = write(1, buf, buf_size);
    num_written = current_written;
    while (current_written != -1 && buf_size-num_written > 0) {
      current_written = write(1, buf, buf_size-num_written);
      num_written += current_written;
    }
    dc1394_dma_done_with_buffer(&camera);
    if (current_written == -1) break;
  }
  close(1);

  /*-----------------------------------------------------------------------
   *  Stop data transmission
   *-----------------------------------------------------------------------*/
  if (dc1394_stop_iso_transmission(handle,camera.node)!=DC1394_SUCCESS) 
  {
    fprintf(stderr, "couldn't stop the camera?\n");
  }

  /*-----------------------------------------------------------------------
   *  Close camera
   *-----------------------------------------------------------------------*/
  dc1394_dma_unlisten(handle, &camera);
  dc1394_dma_release_camera(handle,&camera);
  dc1394_destroy_handle(handle);
  return 0;
}