specap.cpp

// specap.cpp
// ==========
//
// Capture a spectrum with the Little Garden spectrometer on Linux.
// The sensor in this is an HD 1920x1080 UVC compatible camera
// from which the Bayer filter has been removed.

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <ctype.h>
#include <math.h>
#include <fcntl.h>
#include <unistd.h>
#include <errno.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>

#include <linux/videodev2.h>

#include <vector>

#define CLEAR(x) memset(&(x), 0, sizeof(x))

// Error codes.
static const int CAP_E_ALREADY_OPEN = 1;
static const int CAP_E_CANNOT_GET_FORMAT = 2;
static const int CAP_E_CANNOT_OPEN = 3;
static const int CAP_E_CANNOT_SET_FORMAT = 4;
static const int CAP_E_CANNOT_STAT_DEVICE_NAME = 5;
static const int CAP_E_CAPABILITY_QUERY_FAILED = 6;
static const int CAP_E_CONTROL_DISABLED = 7;
static const int CAP_W_DATA_CLIPPED = 8;
static const int CAP_E_DEQUEUE_BUFFER_FAIL = 9;
static const int CAP_E_FMT_BUFFER_MISMATCH = 10;
static const int CAP_E_FMT_NOT_VIDEO_CAPTURE = 11;
static const int CAP_E_INVALID_BUFFER_INDEX = 12;
static const int CAP_E_IOCTL_ERROR = 13;
static const int CAP_E_MMAP_BUFFER_FAIL = 14;
static const int CAP_E_NO_MMAP = 15;
static const int CAP_E_NO_SUCH_CONTROL = 16;
static const int CAP_E_NOT_A_DEVICE = 17;
static const int CAP_E_NOT_A_V4L2_DEVICE = 18;
static const int CAP_E_NOT_CAPTURE_DEVICE = 19;
static const int CAP_E_NOT_OPEN = 20;
static const int CAP_E_QUERY_BUFFER_FAIL = 21;
static const int CAP_E_QUEUE_BUFFER_FAIL = 22;
static const int CAP_E_REQUEST_BUFFERS_FAIL = 23;
static const int CAP_E_SELECT_FAIL = 24;
static const int CAP_E_SELECT_TIMEOUT = 25;
static const int CAP_E_STREAM_OFF_FAIL = 26;
static const int CAP_E_STREAM_ON_FAIL = 27;
static const int CAP_E_UNEXPECTED_BYTESPERLINE = 28;
static const int CAP_E_VIDEO_BUFS_ALLOC_FAIL = 29;
static const int CAP_E_FILE_CREATE_FAIL = 30;
static const int CAP_E_CORRECTION_FILE_FAIL = 31;
static const int CAP_E_WRONG_CAMERA = 32;
static const int CAP_E_CONTROL_NOT_FOUND = 33;

// Control names. Some of these vary with UVC version, it seems.
// The ANCIENT option is for UVC on Ubuntu 14.04.
// The default is what works on Debian 12 (bookworm).
// It is possible these will be different on other Linux versions.
// Use: v4l2-ctl -L to list the control names on a system and edit if necessary.
#ifdef ANCIENT
static const char* CONTRAST = "Contrast";
static const char* SHARPNESS = "Sharpness";
static const char* WBAUTO = "White Balance Temperature, Auto";
static const char* EXPAUTO = "Exposure, Auto";
static const char* EXPTIME = "Exposure (Absolute)";
#else
static const char* CONTRAST = "Contrast";
static const char* SHARPNESS = "Sharpness";
static const char* WBAUTO = "White Balance, Automatic";
static const char* EXPAUTO = "Auto Exposure";
static const char* EXPTIME = "Exposure Time, Absolute";
#endif

// Device control data.
typedef struct {
  char name[80];     // Name.
  unsigned int id;   // Id to which name maps.
} ctrl_info;

// Global data.
static const int max_ctrls = 16;                // Maximum number of controls for known camera type.
static int fd = -1;                             // File descriptor for device.
static struct v4l2_capability cap;              // Device capabilities.
static struct v4l2_format fmt;                  // Image format.
static ctrl_info ctrls_table[max_ctrls];        // Array of camera control descriptions.
static int num_ctrls = 0;                       // Number of controls found.

static int ioctl_nointr(int fh, int request, void *arg)
//-----------------------------------------------------
//  Perform ioctl() across interrupted system calls.
//
//  fh Device file handle.
//  request Request code.
//  arg Pointer to data for request.
//  Return -1 on error (which the caller may expect) else 0 or a return value.
{
  int r;

  do {
    r = ioctl(fh, request, arg);
  } while (-1 == r && EINTR == errno);

#if DEBUG
  if( r == -1 )
    perror("capture:ioctl_nointr():");
#endif
  return r;
}

int uvc_set_control( unsigned int id, int value )
//-----------------------------------------------
//  Set the control with specified id to value.
// The device must have been opened at this point. 
// The id value should be for a control known to exist.
//
//  id Control id.
//  value Value to set control to.
//  Return Error code. 0 if OK.
{
  struct v4l2_queryctrl queryctrl;
  struct v4l2_control control;

  // Ensure open.
  if( fd < 0 )
    return CAP_E_NOT_OPEN;

  CLEAR(queryctrl);
  queryctrl.id = id;

  // See if the control exists.
  if (-1 == ioctl(fd, VIDIOC_QUERYCTRL, &queryctrl)) {
    if (errno != EINVAL)
      return CAP_E_NO_SUCH_CONTROL;
    else
      return CAP_E_IOCTL_ERROR;
  }

  // And is not disabled ...
  if (queryctrl.flags & V4L2_CTRL_FLAG_DISABLED)
    return CAP_E_CONTROL_DISABLED;

  // Set the value then ...
  //printf("uvc_set_control(%u): control type code = %d\n",id,(int)queryctrl.type);
  //printf("uvc_set_control(%u): flags = %0x\n",id,queryctrl.flags);
  CLEAR(control);
  control.id = id;
  control.value = value;

  if (-1 == ioctl_nointr(fd, VIDIOC_S_CTRL, &control))
    return CAP_E_IOCTL_ERROR;

  return 0;
}

int uvc_get_control( unsigned int id, int& value )
//-----------------------------------------------
//  Get the value of the control with specified id.
// The device must have been opened at this point. 
// The id value should be for a control known to exist.
//
//  id Control id.
//  value Value to set control to.
//  Return Error code. 0 if OK.
{
  struct v4l2_control control;

  // Ensure open.
  if( fd < 0 )
    return CAP_E_NOT_OPEN;

  // Set default return value.
  value = 0;

  CLEAR(control);
  control.id = id;

  // Try setting the control.
  if( 0 == ioctl(fd, VIDIOC_G_CTRL, &control) )
    value = control.value;
  
  // A problem ...
  else{
    if( errno != EINVAL )
      return CAP_E_NO_SUCH_CONTROL;
    else
      return CAP_E_IOCTL_ERROR;
  }

  return 0;
}

int uvc_open_device( const char* devname )
//----------------------------------------
// Open an image capture device: i.e. a "webcam".
// Get its capabilities and descriptions of all its controls.
//
//  devname Name of device to use. E.g. /dev/video0
//
// Return Error code. 0 if no error.
{
  struct stat st;

  // Can only have one device open at a time.
  if( fd != -1 )
    return CAP_E_ALREADY_OPEN;

  // Stat the supposed device.
  if( -1 == stat(devname, &st) )
    return CAP_E_CANNOT_STAT_DEVICE_NAME;

  // Ensure it is a character device.
  if( !S_ISCHR(st.st_mode) )
    return CAP_E_NOT_A_DEVICE;

  // Open the device.
  fd = open(devname, O_RDWR | O_NONBLOCK, 0);
  if( -1 == fd )
    return CAP_E_CANNOT_OPEN;

  // Get the capabilities of the device.
  if( -1 == ioctl_nointr(fd, VIDIOC_QUERYCAP, &cap) ){
    close( fd );
    fd = -1;
    if( EINVAL == errno )
      return CAP_E_NOT_A_V4L2_DEVICE;
    else
      return CAP_E_CAPABILITY_QUERY_FAILED;
  }

  // Make sure it can capture video!
  if( !(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE) ){
    close( fd );
    fd = -1;
    return CAP_E_NOT_CAPTURE_DEVICE;
  }

  // Get the current image format.
  CLEAR(fmt);
  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if( -1 == ioctl_nointr(fd, VIDIOC_G_FMT, &fmt) ){
    close( fd );
    fd = -1;
    return CAP_E_CANNOT_GET_FORMAT;
  }

  // If the format as returned is not V4L2_BUF_TYPE_VIDEO_CAPTURE, give up.
  if( fmt.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ){
    close( fd );
    fd = -1;
    return CAP_E_FMT_NOT_VIDEO_CAPTURE;
  }

  return 0;
}

int uvc_close_device( void )
//--------------------------
// Close the image capture device. Free resources.
//
// Return Error code. 0 if OK.
{
  if( fd == -1 )
    return CAP_E_NOT_OPEN;
  close( fd );
  fd = -1;
  return 0;
}

int uvc_set_image_size( int width, int height )
//---------------------------------------------
// Set the size of the image to acquire. This must be a supported size, of course.
// Also set the pixel format to YUYV. The device MUST support this.
// Also request progressive (frame based) data.
//
//  width Image width (pixels)
//  height Image  height (lines)
//
// Return Error code. 0 if OK.
{
  if( fd == -1 )
    return CAP_E_NOT_OPEN;

  // Set the desired image format.
  CLEAR(fmt);
  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  fmt.fmt.pix.width = width;
  fmt.fmt.pix.height = height;
  fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
  fmt.fmt.pix.field       = V4L2_FIELD_NONE;
  if( -1 == ioctl_nointr(fd, VIDIOC_S_FMT, &fmt) ){
    close( fd );
    fd = -1;
    return CAP_E_CANNOT_SET_FORMAT;
  }

  return 0;
}

static int uvc_fetch_controls( void )
//-----------------------------------
/// @brief Fetch minimal information on all the controls. Used to get control id from string.
///
/// @return Error code. 0 if OK.
{
  struct v4l2_queryctrl queryctrl;
  unsigned int id;

  // Ensure open.
  if( fd < 0 )
    return CAP_E_NOT_OPEN;

  CLEAR(queryctrl);

  // Enumerate the controls. Two different methods seem to be required
  // to get them all. First method ...
  queryctrl.id = V4L2_CTRL_FLAG_NEXT_CTRL;
  while( ioctl(fd, VIDIOC_QUERYCTRL, &queryctrl) == 0 ){
    if( num_ctrls < max_ctrls ){
      strncpy(ctrls_table[num_ctrls].name, (const char*)queryctrl.name, 80);
      ctrls_table[num_ctrls].id = queryctrl.id;
      ++num_ctrls;
    }
    queryctrl.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
  }
  
  if( queryctrl.id != V4L2_CTRL_FLAG_NEXT_CTRL ){
    return 0;
  }

  // Second method, standard controls.
  for( id = V4L2_CID_USER_BASE; id < V4L2_CID_LASTP1; id++ ){
    queryctrl.id = id;
    if (ioctl(fd, VIDIOC_QUERYCTRL, &queryctrl) == 0){
      if( num_ctrls < max_ctrls ){
        strncpy(ctrls_table[num_ctrls].name, (const char*)queryctrl.name, 80);
        ctrls_table[num_ctrls].id = queryctrl.id;
        ++num_ctrls;
      }
    }
  }

  // Second method, private controls.
  for( queryctrl.id = V4L2_CID_PRIVATE_BASE; ioctl(fd, VIDIOC_QUERYCTRL, &queryctrl) == 0; queryctrl.id++ ){
    if( num_ctrls < max_ctrls ){
      strncpy(ctrls_table[num_ctrls].name, (const char*)queryctrl.name, 80);
      ctrls_table[num_ctrls].id = queryctrl.id;
      ++num_ctrls;
    }
  }

  return 0;
}

static int uvc_list_controls( void )
//----------------------------------
// List control names and id-s that have been found.
{
  printf("Controls list\n");
  printf("-------------\n");
  for( int i=0; i<num_ctrls; i++ ){
    printf("%2d: [%s] id=%u\n", i, ctrls_table[i].name, ctrls_table[i].id);
  }
  return 0;
}

static unsigned int uvc_get_ctrl_id(const char* name)
//---------------------------------------------------
// Return the control id given a control name.
// Control id-s vary across systems, it seems.
{
  for( int i=0; i<num_ctrls; i++ ){
    //fprintf(stderr,"[%s]=%u\n",ctrls_table[i].name,ctrls_table[i].id);
    if( ! strcmp(ctrls_table[i].name, name) )
      return ctrls_table[i].id;
  }

  fprintf(stderr, "Internal error: control name not found.\n");
  exit(CAP_E_CONTROL_NOT_FOUND);
}

static int process_image( const void *vp, int size, float* values, int w, int h, float clip_limit, bool full_range )
//------------------------------------------------------------------------------------------------------------------
// Convert Y'CrCb data (V4L2_COLORSPACE_SRGB=8) in vp in to normalized floating point linear
// Y data in values. The sensor data will have had some OETF applied: i.e. it will be non-linear
// and suitable for direct display on an sRGB monitor. The exact OETF is unknown, but
// it seems to be the inverse of the sRGB EOTF, which I find surprising, because increased contrast
// is usually desirable (dependent on ambient light levels ideally), rather than linear scene light
// to linear monitor light.
//
// NOTE that the sensor in the Little Garden Spectrometer has had its Bayer filter removed,
// so only the luminance samples are valid.
//
// NOTE the data should *not* be *full range*: i.e. the values *should* be in the ranges
// ([16:235],[16:240],[16:240]). Just in case they are not (!) the full_range argument allows
// [0:255] to be used instead. It *seems* that the LGS camera on recent Linux versions does
// return values in the correct range (but some earlier UVC versions definitely did not.
//
// Return clipping status: 1 if clipped, 0 if not.
//
//  vp Pointer to raw data buffer (from UVC driver).
//  size Number of bytes in buffer vp points to.
//  values Pointer to buffer to receive luminance data.
//  w Width of image (pixels).
//  h Height of image (lines).
//  clip_limit Value (sensor) which, if exceeded, is considered to indicate clipped data.
//  full_range if data is thought to be in the "wrong" range ...
//
// Return 0 if unclipped else 1.
{
  float y1, y2;
  int ip, op;
  unsigned char* p = (unsigned char*) vp;
  float vt, vmin, vscale;
  float vmax = 0.0f;

  if( full_range ){
    vmin = 0.0f;
    vscale = 1.0f / 255.0f;
  }
  else{
    vmin = 16.0f;
    vscale = 1.0f / (235.0f - 16.0f);
  }
  
  printf( "... got %d bytes at address %p ... ", size, p );
  for( ip=0,op=0; ip<size; ip+=4,op+=2 ){    

    y1 = vscale * (p[ip] - vmin);
    y2 = vscale * (p[ip+2] - vmin);

    vt = fmaxf(0.0f, fminf(1.0f, y1));
    vmax = fmaxf(vmax, vt);
    values[op] += vt;

    vt = fmaxf(0.0f, fminf(1.0f, y2));
    vmax = fmaxf(vmax, vt);
    values[op+1] += vt;
  }

  printf("max value: %.3f, clip_limit: %.3f\r", vmax, clip_limit );
  fflush(stdout);
  if( (vmax < clip_limit) )
    return 0;
  else
    return 1;
}

int uvc_capture( float* values, int out_width, int out_height, int navg, float clip_limit, bool full_range )
//----------------------------------------------------------------------------------------------------------
// Capture a single frame of luminance and put it in values. The expected size
// of the result, for which memory will be assumed to have been allocated by the caller,
// is out_width by out_height.
//
// This can internally average over multiple frame captures. The number of captured frames
// is navg (actually, navg+2 and the first 2 thrown away to work around what seem to be "issues"
// with some cameras).
//
//  values Pointer to buffer to receive luminance data.
//  out_width Expected image width (for which values has been allocated).
//  out_height Expected image height (for which values has been allocated).
//  navg Number of frames to capture and average.
//  clip_limit Normalized image value which, if exceeded, will cause CAP_W_DATA_CLIPPED to be returned.
//  full_range Take camera sensor data to be in [0:255] rather than the "correct" range [16:235] for Y.
//
// Return Error code. 0 if OK, clipped data: CAP_W_DATA_CLIPPED, or an error code.
{
  enum v4l2_buf_type type;
  int ret_status = 0;

  // Zero the output image.
  memset( values, 0, out_width * out_height * sizeof(float) );
  
  // Check device is open.
  if( fd == -1 )
    return CAP_E_NOT_OPEN;
  int fd_flags = fcntl(fd, F_GETFL);

  // Find out if the camera is in manual exposure mode. If so, we need to do some tricks
  // later to ensure the images are captured with the set exposure value.
  bool manual_exposure_mode = false;
  int exposure_mode = 0;
  if( uvc_get_control( V4L2_CID_EXPOSURE_AUTO, exposure_mode ) == 0 ){
    manual_exposure_mode = ( exposure_mode == V4L2_EXPOSURE_MANUAL );
    //printf("!!! set manual exposure mode.\n");
  }

  // Get the current image format. Needed for expected image size.
  // The format should have been set to YUYV at a specific size by a previous call to uvc_set_image_size().
  CLEAR(fmt);
  fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  if( -1 == ioctl_nointr(fd, VIDIOC_G_FMT, &fmt) )
    return CAP_E_CANNOT_GET_FORMAT;
  int width = fmt.fmt.pix.width;
  int height = fmt.fmt.pix.height;
  int bytesperline = fmt.fmt.pix.bytesperline;
  if( bytesperline != 2 * width )
    return CAP_E_UNEXPECTED_BYTESPERLINE;
  //printf( "field format is: %d\n",fmt.fmt.pix.field );

  // Check that matches the expected size.
  if( width != out_width || height != out_height )
    return CAP_E_FMT_BUFFER_MISMATCH;

  // See if we can use memory mapped i/o.
  // Set the number of buffers we want to have available.
  struct v4l2_requestbuffers reqbuf;
  memset(&reqbuf, 0, sizeof(reqbuf));
  reqbuf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  reqbuf.memory = V4L2_MEMORY_MMAP;
  reqbuf.count = navg;

  if (-1 == ioctl (fd, VIDIOC_REQBUFS, &reqbuf)) {
    if (errno == EINVAL)
      return CAP_E_NO_MMAP;
    else
      return CAP_E_REQUEST_BUFFERS_FAIL;
  }

  typedef struct {
    void *start;
    size_t length;
  } VIDBUF;

  VIDBUF* buffers;

  // Try allocating the buffer descriptors and mapping them.
  buffers = (VIDBUF*)calloc(reqbuf.count, sizeof(VIDBUF));
  if( buffers == NULL )
    return CAP_E_VIDEO_BUFS_ALLOC_FAIL;

  // Get valid buffer pointers via MMAP.
  else{
    unsigned int i;
    for (i = 0; i < reqbuf.count; i++) {
      struct v4l2_buffer buffer;

      memset(&buffer, 0, sizeof(buffer));
      buffer.type = reqbuf.type;
      buffer.memory = V4L2_MEMORY_MMAP;
      buffer.index = i;

      // Get buffer information here. Give up if fails.
      if (-1 == ioctl (fd, VIDIOC_QUERYBUF, &buffer)) {
	for( unsigned int j=0; j<i; j++ )
	  munmap(buffers[j].start, buffers[j].length);
	free( buffers );
	return CAP_E_QUERY_BUFFER_FAIL;
      }

      buffers[i].length = buffer.length; /* remember for munmap() */
      buffers[i].start = mmap(NULL, buffer.length,
			      PROT_READ | PROT_WRITE, /* recommended */
			      MAP_SHARED,             /* recommended */
			      fd, buffer.m.offset);

      if ( MAP_FAILED == buffers[i].start ) {
	for( unsigned int j=0; j<i; j++ )
	  munmap(buffers[j].start, buffers[j].length);
	free( buffers );
	return CAP_E_MMAP_BUFFER_FAIL;
      }
    } // Over buffers.

    // Start capturing.
    // Queue all the video frame buffers.
    for( i = 0; i < reqbuf.count; ++i ){
      struct v4l2_buffer buf;

      CLEAR(buf);
      buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
      buf.memory = V4L2_MEMORY_MMAP;
      buf.index = i;

      if (-1 == ioctl_nointr(fd, VIDIOC_QBUF, &buf)){
	for( unsigned int j=0; j<reqbuf.count; j++ )
	  munmap(buffers[j].start, buffers[j].length);
	free( buffers );
	return CAP_E_QUEUE_BUFFER_FAIL;
      }
    }

    // Turn on streaming.
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (-1 == ioctl_nointr(fd, VIDIOC_STREAMON, &type)){
      for( unsigned int j=0; j<reqbuf.count; j++ )
	munmap(buffers[j].start, buffers[j].length);
      free( buffers );
      return CAP_E_STREAM_ON_FAIL;
    }

    // Get frames from the camera.
    // Actually get two more than requested ... sigh ... It seems that there is no guarantee that certain
    // control settings related to getting an image will be in effect on the first frame captured after setting them.
    // Or, maybe, ever (without force)! These controls certainly include exposure_absolute --
    // at least on the cameras tested so far.
    unsigned int count = navg + 2;
    while (count-- > 0) {

      // If we are in manual exposure mode and doing the two initial (discarded) frames, get the
      // exposure value and set it again. Ramming this value repeatedly home seems to be the only way
      // to get the exposure value that has been set. Certainly the case for the YW-1080 camera
      // (Z-Star Microelectronics Corp. Venus USB2.0 Camera). Neither v4l2-ctl nor guvcview can reliably
      // set the exposure on this camera either, so this workaround is essential (and unique).
      if( manual_exposure_mode && ((int)count >= navg) ){
        int value;
        uvc_get_control(V4L2_CID_EXPOSURE_ABSOLUTE,value);
        uvc_set_control(V4L2_CID_EXPOSURE_ABSOLUTE,value);
      }

      // Loop waiting for a frame (blocks on select()).
      for (;;) {
	fd_set fds;
	struct timeval tv;
	int r;

	FD_ZERO(&fds);
	FD_SET(fd, &fds);

	// Set a timeout ... do not wait forever for a frame.
	// Wait for a buffer to be available.
	tv.tv_sec = 12;
	tv.tv_usec = 0;

	r = select(fd + 1, &fds, NULL, NULL, &tv);

	// Possibly an error on select() ... 
	if (-1 == r) {
	  if (EINTR == errno)
	    continue; // Interrupted system call is fine.
	  for( unsigned int j=0; j<reqbuf.count; j++ )
	    munmap(buffers[j].start, buffers[j].length);
	  free( buffers );
	  return CAP_E_SELECT_FAIL;
	}

	// Possibly a time out on select() ...
	if (0 == r) {
	  for( unsigned int j=0; j<reqbuf.count; j++ )
	    munmap(buffers[j].start, buffers[j].length);
	  free( buffers );
	  return CAP_E_SELECT_TIMEOUT;
	}

	// Read the frame data.
	{
	  struct v4l2_buffer buf;
	  CLEAR(buf);
	  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	  buf.memory = V4L2_MEMORY_MMAP;

	  // De-queue a buffer. It should contain the video data.
	  if (-1 == ioctl_nointr(fd, VIDIOC_DQBUF, &buf)) {
	    switch (errno) {
	    case EAGAIN:
	      continue;  // Interrupted system call (EAGAIN). Continue select() loop.

	    case EIO:
	      // Could ignore EIO, see spec.
	      /* fall through */

	    default:
	      for( unsigned int j=0; j<reqbuf.count; j++ )
		munmap(buffers[j].start, buffers[j].length);
	      free( buffers );
	      return CAP_E_DEQUEUE_BUFFER_FAIL;
	    }
	  }

	  // Ensure the buffer index is valid.
	  if( buf.index >= reqbuf.count ){
	    for( unsigned int j=0; j<reqbuf.count; j++ )
	      munmap(buffers[j].start, buffers[j].length);
	    free( buffers );
	    return CAP_E_INVALID_BUFFER_INDEX;
	  }

          // If this is a buffer we think is reliably exposed, process it.
          // Convert it to RGB and add the result to the output buffer.
          // Detect apparent clipping in RGB.
          if( (int)count < navg ){
            //printf("Count: %d\n",count);fflush(stdout);
              
            // Convert the image data to floating point RGB in planes. Add to average.
            if( process_image(buffers[buf.index].start, buf.bytesused,
                              values, out_width, out_height, clip_limit, full_range) != 0 ){
              ret_status = CAP_W_DATA_CLIPPED;
            }
          }

	  // Re-queue this buffer to be re-filled.
	  if (-1 == ioctl_nointr(fd, VIDIOC_QBUF, &buf)){
	    for( unsigned int j=0; j<reqbuf.count; j++ )
	      munmap(buffers[j].start, buffers[j].length);
	    free( buffers );
	    return CAP_E_QUEUE_BUFFER_FAIL;
	  }

	  break; // Exit select() loop.
	}
        
      } // select() loop.
    } // frame count loop.

    // Stop capturing.
    // Turn off streaming.
    type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (-1 == ioctl_nointr(fd, VIDIOC_STREAMOFF, &type)){
      for( unsigned int j=0; j<reqbuf.count; j++ )
	munmap(buffers[j].start, buffers[j].length);
      free( buffers );
      return CAP_E_STREAM_OFF_FAIL;
    }

    // Unmap buffers and free them.
    for (i = 0; i < reqbuf.count; i++)
      munmap(buffers[i].start, buffers[i].length);
    free( buffers );
  } // Allocated video buffer descriptors.

  // Complete the averaging of the image data.
  float rnavg = 1.0f / navg;
  for( int i=0; i<(out_width*out_height); i++ )
    values[i] *= rnavg;

  printf("\n");
  fcntl(fd, F_SETFL, fd_flags);
  return ret_status;
}

void valid_line_map( float* data, float sums[1080] )
//--------------------------------------------------
// Project data horizontally and and try to identify lines
// with significant signal.
{
  float avg = 0.0f;
  float norm = 0.0f;

  for( int k=0; k<1080; k++ ){
    float sum = 0.0f;
    for( int i=0; i<1920; i++ )
      sum += data[k*1920+i];
    sums[k] = sum;
    avg += sum;
  }

  avg /= 1080.0f;
  for( int k=0; k<1080; k++ ){
    float v = sums[k] - avg;
    v = fmaxf(v, 0.0f);
    if( v > norm )norm = v;
    sums[k] = v;
  }

  for( int k=0; k<1080; k++ )
    sums[k] /= norm;
}

static inline float sRGB_OETF( float x )
//--------------------------------------
// Inverse of the sRGB EOTF defined in: IEC 61966-2-1:1999 (sRGB)
// It seems that this works well for linearising the sensor values
// of the camera used in the Little Garden Spectrometer (which I find
// a bit surprising for ... reasons).
{
  static const float xb = 0.0392857119f;
  static const float fs = 0.0773801506f;
  static const float offs = 0.055f;
  static const float offs1 = offs + 1.0f;
  static const float gamma = 2.4f;
  float y;
  
  if( x >= xb ) 
    y = powf((x + offs) / offs1, gamma);
  else
    y = x * fs;
  
  return y;
}

int write_spectrum( const char* filename, float* data, bool pixelmode, bool correct, int exp, float clip_limit, bool verbose )
//----------------------------------------------------------------------------------------------------------------------------
// Interpret an image as a spectrum. Write it to a simple text file.
// Data format is <wavelength> <relative intensity>, one line per wavelength, both
// as floating point numbers.
// Wavelength range: 350 nm to 850 nm, 922 samples. ~0.5434 nm/step.
//
//  filename:  Output file name.
//  data: Image as 1920x1080 floats, normalised to 1.0 for non-linear luminance pixel value 255.
//  pixelmode: If true, output raw data for all pixels with pixel number instead of wavelength.
//             This is used when obtaining data for wavelength calibration.
//  correct: If true, use amplitude correction data stored in "correction.txt".
//             This should be used unless a new correction is being measured.
//  exp: Exposure time (unknown units) used to select an amplitude correction file.
//  clip_limit: non-linear but normalised sensor value at which sensor clips.
//  verbose: Output information.
//
// Return Error code. 0 if OK, or an error code.
{
  float spectrum[1920], linemap[1920];
  float corr = 1.0f;

  // Wavelength calibration using a mercury based fluorescent lamp.
  // The constants are found using the wavelength_cal.py program using
  // data from specap in pixelmode.
  // Linear scale found from fitting peak wavelengths to pixel numbers.
  // These values are found by the wavelength_cal.py program.
  float pixel_to_wavelength_slope = 0.542998285f;
  // Linear offset found from fitting peak wavelengths to pixel numbers.
  float pixel_to_wavelength_intercept = 241.097587f;
  
  // Set these after doing a wavelength calibration in "pixelmode".
  int min_350_nm_pixel = 200;  // Nearest pixel number corresponding to 350 nm.
  int max_850_nm_pixel = 1122; // Nearest pixel number corresponding to 850 nm.

  // See if updated wavelength calibration data exists and read it if so.
  {
    FILE* fincal = fopen("/usr/local/lgscli/active_calibration.txt", "r");
    if( NULL != fincal ){
      float m, c;
      int p350, p850, nread;
      nread = fscanf(fincal, "%f %f %d %d", &m, &c, &p350, &p850);
      if( 4 == nread ){
        pixel_to_wavelength_slope = m;
        pixel_to_wavelength_intercept = c;
        min_350_nm_pixel = p350;
        max_850_nm_pixel = p850;
        if( verbose )
          printf("Read updated wavelength calibration data from /usr/local/lgscli/active_calibration.txt\n");
      }
      fclose(fincal);
    }
    else{
      if( verbose )
        printf("NOTE: Did not read wavelength calibration data from /usr/local/lgscli/active_calibration.txt\n");
    }
  }
  if( verbose ){
    printf("pixel_to_wavelength_slope = %f\n", pixel_to_wavelength_slope);
    printf("pixel_to_wavelength_intercept = %f\n", pixel_to_wavelength_intercept);
    printf("min_350_nm_pixel = %d\n", min_350_nm_pixel);
    printf("max_850_nm_pixel = %d\n", max_850_nm_pixel);
  }

  // Convert sensor values to linear values.
  // It turns out that the results are quite sensitive to this. In the end, the inverse
  // sRGB OETF seems to work best, after having tried power functions with many values.
  // To check this, generate a correction file with a tungsten lamp, measure a spectrum with
  // a daylight LED lamp, calculate the CCT and look at the shape of the spectrum. The CCT
  // should be fairly close to expected (but a 6500K lamp may be as low as 6100K) and the
  // shape should look like other 6500K lamps as measured here:
  // https://data.dtu.dk/articles/dataset/EMPIR_15SIB07_PhotoLED_-_Database_of_LED_product_spectra/12783389.
  // Note that using the correct CCT for the tungsten lamp when generating the correction file
  // is very important! A 60W Philips bulb is 2600K. A 12V 50W tungsten halogen bead lamp is 3000K.
  //
  // Linearised clipping value. Normalise linear value so this is 1.0.
  const float linear_clip = sRGB_OETF(clip_limit);

  // Open the correction scale data file, unless correct is false.
  // If not found, treat that as a warning only.
  FILE* fin = NULL;
  if( correct ){
    fin = fopen("/usr/local/lgscli/active_correction.txt", "r");
    if( NULL == fin ){
      fprintf(stderr, "WARNING: Cannot open /usr/local/lgscli/active_correction.txt\n");
    }
    else{
      if( verbose )
        fprintf(stderr, "Reading correction data from /usr/local/lgscli/active_correction.txt\n");
    }
  }
  else{
    if( verbose )
      fprintf(stderr, "Not using correction data.\n");
  }
  
  // Create the output spectrum data file.
  FILE* fout = fopen(filename, "w");
  if( NULL == fout )
    return CAP_E_FILE_CREATE_FAIL;

  // Try to identify lines with useful data.
  valid_line_map(data, linemap);

  // Average lines with useful data.
  int navg = 0;
  for( int i=0; i<1920; i++ )
    spectrum[i] = 0.0f;
  
  for( int k=0; k<1080;k++ ){
    if( linemap[k] >= 0.8f ){
      ++navg;
      for( int i=0; i<1920; i++ )
        spectrum[i] += data[k*1920+i];
    }
  }

  if( navg == 0 )
    navg = 1;
  for( int i=0; i<1920; i++ )
    spectrum[i] /= float(navg);

  // Write data for "raw pixel" mode so wavelength calibration can be done.
  // No linearisation or amplitude correction.
  if( pixelmode ){
    for( int i=0; i<1920; i++ ){
      int j = 1919 - i;  // Flip left-right.
      float v = spectrum[j];
      fprintf(fout, "%f %f\n", float(i), v);
    }
  }
  
  // Write data corresponding to the (extended) visible wavelength range.
  else{
    for( int i=min_350_nm_pixel; i<max_850_nm_pixel; i++ ){
      int j = 1919 - i;  // Flip left-right.
      float v = spectrum[j];
      
      // Convert the X pixel number to wavelength.
      float f = pixel_to_wavelength_slope * i + pixel_to_wavelength_intercept;

      // Linearise (hopefully).
      float vcorr = sRGB_OETF(v);

      // Apply amplitude correction, if available.
      if( NULL != fin ){
        if( fscanf(fin, "%f", &corr) == 1 )
          vcorr *= corr;
        else
          return CAP_E_CORRECTION_FILE_FAIL;
      }

      // Scale so linearised clip limit is 1.0.
      vcorr /= linear_clip;

      // Write wavelength and value to output file.
      fprintf(fout, "%f %f\n", f, vcorr);
    }
  } // Calibrated mode.

  // Clean up.
  fclose(fout);

  if( NULL != fin )
    fclose(fin);
  
  return 0;
}

void check_status( int ecode )
//----------------------------
//  If ecode is not zero, output a message to stderr then exit with condition code ecode.
//
//  ecode Error code.
{
  const char* msg = NULL;
  
  if( 0 == ecode )
    return;
  
  switch(ecode){
  case CAP_E_ALREADY_OPEN: msg = "An image capture device is already open."; break;
  case CAP_E_CANNOT_STAT_DEVICE_NAME: msg = "Device not found (cannot stat())."; break;
  case CAP_E_NOT_A_DEVICE: msg = "Device name is not that of a device."; break;
  case CAP_E_CANNOT_OPEN: msg = "Cannot open the device."; break;
  case CAP_E_NOT_A_V4L2_DEVICE: msg = "Device not supported by V4L2."; break;
  case CAP_E_CAPABILITY_QUERY_FAILED: msg = "Could not get device capabilities."; break;
  case CAP_E_NOT_CAPTURE_DEVICE: msg = "The device cannot capture video."; break;
  case CAP_E_NOT_OPEN: msg = "No device is open."; break;
  case CAP_E_CANNOT_GET_FORMAT: msg = "Cannot get image format."; break;
  case CAP_E_FMT_NOT_VIDEO_CAPTURE: msg = "Image format is not V4L2_BUF_TYPE_VIDEO_CAPTURE."; break;
  case CAP_E_CANNOT_SET_FORMAT: msg = "Cannot set image format."; break;
  case CAP_E_NO_SUCH_CONTROL: msg = "Specified control does not exist."; break;
  case CAP_E_IOCTL_ERROR: msg = "Unspecified ioctl() error."; break;
  case CAP_E_CONTROL_DISABLED: msg = "Control is disabled."; break;
  case CAP_E_NO_MMAP: msg = "MMAP i/o method not available."; break;
  case CAP_E_REQUEST_BUFFERS_FAIL: msg = "Request i/o buffers failed."; break;
  case CAP_E_VIDEO_BUFS_ALLOC_FAIL: msg = "Failed to allocate video buffers."; break;
  case CAP_E_QUERY_BUFFER_FAIL: msg = "Query video buffer failed."; break;
  case CAP_E_MMAP_BUFFER_FAIL: msg = "MMAP video buffer failed."; break;
  case CAP_E_QUEUE_BUFFER_FAIL: msg = "Queue video buffer failed."; break;
  case CAP_E_STREAM_ON_FAIL: msg = "Failed to turn streaming on."; break;
  case CAP_E_SELECT_FAIL: msg = "select() call failed."; break;
  case CAP_E_SELECT_TIMEOUT: msg = "select() call timed out."; break;
  case CAP_E_DEQUEUE_BUFFER_FAIL: msg = "Failed to de-queue video buffer."; break;
  case CAP_E_INVALID_BUFFER_INDEX: msg = "Invalid buffer index detected."; break;
  case CAP_E_STREAM_OFF_FAIL: msg = "Failed to turn streaming off."; break;
  case CAP_E_FMT_BUFFER_MISMATCH: msg = "Image format and user buffer size mismatch."; break;
  case CAP_E_UNEXPECTED_BYTESPERLINE: msg = "Unexpected format (bytes per line not twice width)."; break;
  case CAP_W_DATA_CLIPPED: msg = "Image data was clipped (too bright)."; break;
  case CAP_E_FILE_CREATE_FAIL: msg = "Could not create output file."; break;
  case CAP_E_CORRECTION_FILE_FAIL: msg = "Error reading correction.txt."; break;
  default: msg = "Unknown error code for image capture. Internal error."; break;
  }
  
  if( ecode == CAP_W_DATA_CLIPPED )
    fprintf(stderr, "WARNING: %s\n", msg);
  else{
    fprintf(stderr, "ERROR: %s\n", msg);
    exit(ecode);
  }
}

int main( int argc, char** argv )
{
  int exp = 200;                   // Default exposure time setting. Units probably 100 microseconds.
  int navg = 16;                   // Number of additional frame averages.
  bool pixelmode = false;          // Output pixel number not wavelength (for wavelength calibration).
  bool correct = true;             // Use amplitude correction data (unless finding a correction curve).
  bool full_range = false;         // Use full range sensor data (which is "wrong", but some ancient UVC does this).
  bool verbose = false;            // Output additional information.

  const float clip_limit = 0.998f; // Limit in normalised non-linear luminance.

  // Option processing data.
  int opt;
  const char* optstring = "hpnvd:f:e:a:s:";
  char numstring[80];
  char devname[80];
  char outname[256];

  // Default device and output file name.
  strncpy(devname, "/dev/video0", sizeof(devname));
  strncpy(outname, "specout.dat", sizeof(outname));

  // Parse command line options.
  while( (opt = getopt(argc, argv, optstring)) != -1 ){
    switch( opt ){
    case 'h':
      fprintf(stderr,
              "Capture a spectrum with the Little Garden Spectrometer.\n"
              "Options: -h : help\n"
              "         -p : select raw pixel mode for wavelength calibration.\n"
              "         -n : do not use amplitude correction data (use if updating correction data).\n"
              "         -b : bug fix for sensor/driver returning incorrect full range values.\n"
              "         -v : verbose informational output.\n"
              "         -d device (/dev/video0) : specify device name.\n"
              "         -f outfileName (specout.dat) : specify output file name.\n"
              "         -s outfileName (specout.dat) : also specify output file name.\n"
              "         -e exposureInt (200 (=20ms)) : specify exposure time.\n"
              "         -a averageFramesInt (16) : number of frames to average over.\n"
              "Reads optional wavelength calibration data from /usr/local/lgscli/active_calibration.txt.\n"
              "Use: wavelength_cal or wavelength_auto_cal programs to generate this from a fluorescent lamp spectrum.\n"
              "Reads amplitude correction data from /usr/local/lgscli/active_correction.txt.\n"
              "Use: correction_cal program to generate this from a tungsten lamp spectrum.\n"
              );
      exit(0);
    case 'p':  // Output X as pixel number, not wavelength.
      pixelmode = true;
      break;
    case 'n':  // Do not use any amplitude correction file.
      correct = false;
      break;
    case 'b':  // Bug fix option.
      full_range = true;
      break;
    case 'v':  // Verbose mode.
      verbose = true;
      break;
    case 'd':  // Specify device name.
      strncpy(devname, optarg, sizeof(devname));
      break;
    case 'f':  // Specify output file name.
    case 's':
      strncpy(outname, optarg, sizeof(outname));
      break;      
    case 'e':  // Specify exposure time.
      strncpy(numstring, optarg, sizeof(numstring));
      exp = atoi(numstring);
      if( exp < 1 )exp = 1; else if( exp > 5000 )exp = 5000;
      break;
    case 'a':  // Specify number of (additional) frame averages.
      strncpy(numstring, optarg, sizeof(numstring));
      navg = atoi(numstring);
      if( navg < 1 )navg = 1; else if( navg > 128 )navg = 128;
      break;
    case '?':  // Something unknown.
      fprintf(stderr, "Usage: %s [-h] [-d device] [-f outfileName] [-e exposureInt] ...\n", argv[0]);
      fprintf(stderr, "Use -h option to get more help.\n");
      exit(EXIT_FAILURE);
    default:   // Option parser failure!
      abort();
    }
  }

  // Image buffer (luminance only).
  std::vector<float> values(1920*1080);

  // Capture a spectrum.
  check_status( uvc_open_device(devname) );
#ifndef ANCIENT
  if( NULL == strstr((char*)cap.card, "USB-ZH") ){
    fprintf(stderr, "Expecting USB-ZH camera for Little Garden spectrometer, but not found.\n");
    return CAP_E_WRONG_CAMERA;
  }
#endif
  if( verbose )
    printf("Found camera: %s\n", (char*)cap.card);

  check_status( uvc_fetch_controls() );
  if( verbose )check_status( uvc_list_controls() );
  check_status( uvc_set_image_size(1920, 1080) );
  check_status( uvc_set_control(uvc_get_ctrl_id(CONTRAST), 20) );  // contrast (32 default).
  check_status( uvc_set_control(uvc_get_ctrl_id(SHARPNESS), 1) );  // sharpness set to minimum
  check_status( uvc_set_control(uvc_get_ctrl_id(WBAUTO), 0) );     // white_balance_automatic false
  check_status( uvc_set_control(uvc_get_ctrl_id(EXPAUTO), 1) );    // auto_exposure Manual Mode
  check_status( uvc_set_control(uvc_get_ctrl_id(EXPTIME), exp) );  // exposure_time_absolute 1 to 5000 (200 ref)
  check_status( uvc_capture(values.data(), 1920, 1080, navg, clip_limit, full_range) );
  check_status( write_spectrum(outname, values.data(), pixelmode, correct, exp, clip_limit, verbose) );
  check_status( uvc_close_device() );
  
  return 0;
}