/* LaHaShem HaAretz U'Mloah */
/* Copyright 2013, 2014, and 2015 Purdue University. All rights reserved. */

/* http://docs.opencv.org/modules/imgproc/doc/geometric_transformations.html */
#define _GNU_SOURCE
/* needed because objects.h undefines _GNU_SOURCE before
   sftlib-c.h includes math.h */
#include <features.h>
#include <math.h>
#include "objects.h"
#undef TRUE
#undef FALSE
#include "sftlib-c.h"

/* variables */

unsigned int threads = MAX_THREADS;
unsigned int frame_number, source, height, width;
int running = FALSE;		/* should be volatile */
int halt = FALSE;
volatile char video[MAX_VIDEO];
Window display_pane;		/* should be volatile */
void *((*task[MAX_THREADS])(void *)) = {&camera_task};
pthread_t thread[MAX_THREADS];
struct task_args task_args[MAX_THREADS];
pthread_mutex_t halt_mutex = PTHREAD_MUTEX_INITIALIZER;
pthread_barrier_t barrier;
double fps;			/* should be volatile */
int undistort = FALSE;		/* should be volatile */
volatile int start_saving_frames = FALSE, saving_frames = FALSE;
volatile int start_playback = FALSE, playback_running = FALSE;
volatile unsigned int frames_to_save = 0;
unsigned int saving_frame_number;
/* hardwired */
/* should be volatile */
double calibration[9] = {
  6.2110748946758451e+02,
  0.,
  3.0921541455463785e+02,
  0.,
  6.1944277297715621e+02,
  2.3086485356415153e+02,
  0.,
  0.,
  1.
};
/* should be volatile */
double distortion[5] = {
  -3.4819866264055271e-01,
  2.7018554622358265e-01,
  -9.2885709991282503e-04,
  4.4016807967088940e-03,
  -2.5230004187983329e-01
};
/* needs work: initializer assumes PIPELINE==2 */
Imlib_Image raw_frame[PIPELINE] = {NULL, NULL};
/* needs work: initializer assumes PIPELINE==2 */
IplImage *bgra[PIPELINE] = {NULL, NULL};

/* Scheme->C */

TSCP bool_tscp(int bool) {
  if (bool) return TRUEVALUE;
  else return FALSEVALUE;
}

int tscp_bool(TSCP bool) {
  if (bool==TRUEVALUE) return 1;
  if (bool==FALSEVALUE) return 0;
  sc_error("TSCP_BOOL", "~s is not a boolean", CONS(bool, EMPTYLIST));
  exit(EXIT_FAILURE);
}

/* Fix Scheme->C signed integers */

TSCP sc_int_tscp(int n) {
  if (n<=MAXTSCPINT&&n>=MINTSCPINT) return C_FIXED(n);
  return MAKEFLOAT((double)n);
}

int sc_tscp_int(TSCP p) {
  switch TSCPTAG(p) {
    case FIXNUMTAG:
      return FIXED_C(p);
      break;
    case EXTENDEDTAG:
      if (TX_U(p)->extendedobj.tag==DOUBLEFLOATTAG) {
        return (S2CINT)FLOAT_VALUE(p);
      }
      break;
    }
  sc_error("TSCP_S2CINT", "Argument cannot be converted to C int",
	   EMPTYLIST);
  return 0;                     /* silences warning */
}

/* my stuff */

void *sft_malloc(size_t size) {
  void *p = malloc(size);
  if (p==NULL) sft_error("Out of memory");
  return p;
}

void set_source(unsigned int new_value) {
  source = new_value;
}

void set_display_pane(Window new_value) {
  display_pane = new_value;
}

double current_time(void) {
  struct timeval time;
  if (gettimeofday(&time, NULL)!=0) sft_error("gettimeofday failed");
  /* needs work: Will it convert division into multiplication? */
  return ((double)time.tv_sec)+((double)time.tv_usec)/1e6;
}

/* ffmpeg */

int ffmpeg_first_video_stream(struct ffmpeg_video *ffmpeg_video) {
  if (av_find_stream_info(ffmpeg_video->pFormatCtx)<0) {
    sft_error("Can't get video stream information");
  }
  /* upper bound */
  for (unsigned int i = 0; i<ffmpeg_video->pFormatCtx->nb_streams; i++) {
    if (ffmpeg_video->pFormatCtx->streams[i]->codec->codec_type==
	AVMEDIA_TYPE_VIDEO) {
      return i;
    }
  }
  sft_error("Can't find first video stream");
}

AVCodecContext *ffmpeg_get_codec(struct ffmpeg_video *ffmpeg_video) {
  AVCodecContext *pCodecCtx =
    ffmpeg_video->pFormatCtx->streams[ffmpeg_video->videoStream]->codec;
  AVCodec *pCodec = avcodec_find_decoder(pCodecCtx->codec_id);
  if (pCodec==NULL) sft_error("Unsupported codec!");
  if (avcodec_open(pCodecCtx, pCodec)<0) sft_error("Can't open codec!");
  return pCodecCtx;
}

int ffmpeg_next_frame(struct ffmpeg_video *ffmpeg_video) {
  av_free_packet(&ffmpeg_video->packet);
  int frameFinished;
  int nextFrameValid =
    av_read_frame(ffmpeg_video->pFormatCtx, &ffmpeg_video->packet)>=0;
  if (nextFrameValid&&
      ffmpeg_video->packet.stream_index==ffmpeg_video->videoStream) {
    avcodec_decode_video2(ffmpeg_video->pCodecCtx,
			  ffmpeg_video->pFrame,
			  &frameFinished,
			  &ffmpeg_video->packet);
    if (frameFinished) ffmpeg_video->frame++;
    else ffmpeg_next_frame(ffmpeg_video);
  }
  else if (nextFrameValid) ffmpeg_next_frame(ffmpeg_video);
  else if (!ffmpeg_video->videoFinished&&!nextFrameValid) {
    /* This is required because ffmpeg hangs on to many frames internally */
    AVPacket packet;
    packet.data = NULL;
    packet.size = 0;
    avcodec_decode_video2(ffmpeg_video->pCodecCtx,
			  ffmpeg_video->pFrame,
			  &frameFinished,
			  &packet);
    if (frameFinished) ffmpeg_video->frame++;
    else ffmpeg_video->videoFinished = TRUE;
  }
  return !ffmpeg_video->videoFinished;
}

int ffmpeg_video_finished(struct ffmpeg_video *ffmpeg_video) {
  return ffmpeg_video->videoFinished;
}

void ffmpeg_close_and_free_video(struct ffmpeg_video *ffmpeg_video) {
  av_free(ffmpeg_video->buffer);
  av_free(ffmpeg_video->pFrameBGRA);
  av_free(ffmpeg_video->pFrame);
  avcodec_close(ffmpeg_video->pCodecCtx);
  av_close_input_file(ffmpeg_video->pFormatCtx);
  av_free_packet(&ffmpeg_video->packet);
  ffmpeg_video->videoFinished = TRUE;
  free(ffmpeg_video);
}

struct ffmpeg_video *ffmpeg_open_video(char *filename) {
  struct ffmpeg_video *ffmpeg_video =
    (struct ffmpeg_video *)sft_malloc(sizeof(struct ffmpeg_video));
  /* needs work: bzero is deprecated, should use memset */
  bzero(ffmpeg_video, sizeof(struct ffmpeg_video));
  av_register_all();
  if (avformat_open_input(&ffmpeg_video->pFormatCtx, filename, NULL, NULL)!=0) {
    sft_error("Can't open video %s", filename);
  }
  ffmpeg_video->videoStream = ffmpeg_first_video_stream(ffmpeg_video);
  ffmpeg_video->pCodecCtx = ffmpeg_get_codec(ffmpeg_video);
  ffmpeg_video->pFrame = avcodec_alloc_frame();
  ffmpeg_video->pFrameBGRA = avcodec_alloc_frame();
  if (!ffmpeg_video->pFrameBGRA||!ffmpeg_video->pFrame) {
    sft_error("Can't allocate frame");
  }
  ffmpeg_video->buffer =
    (uint8_t *)av_malloc(avpicture_get_size(PIX_FMT_BGRA,
					    ffmpeg_video->pCodecCtx->width,
					    ffmpeg_video->pCodecCtx->height)*
			 sizeof(uint8_t));
  avpicture_fill((AVPicture *)ffmpeg_video->pFrameBGRA,
		 ffmpeg_video->buffer, PIX_FMT_BGRA,
		 ffmpeg_video->pCodecCtx->width,
		 ffmpeg_video->pCodecCtx->height);
  ffmpeg_video->img_convert_ctx =
    sws_getContext(ffmpeg_video->pCodecCtx->width,
		   ffmpeg_video->pCodecCtx->height,
		   ffmpeg_video->pCodecCtx->pix_fmt,
		   ffmpeg_video->pCodecCtx->width,
		   ffmpeg_video->pCodecCtx->height,
		   PIX_FMT_BGRA, SWS_BICUBIC,
		   NULL, NULL, NULL);
  ffmpeg_video->videoFinished = FALSE;
  ffmpeg_video->frame = 0;
  av_init_packet(&ffmpeg_video->packet);
  ffmpeg_next_frame(ffmpeg_video);
  return ffmpeg_video;
}

Imlib_Image ffmpeg_get_frame_as_imlib(struct ffmpeg_video *ffmpeg_video) {
  sws_scale(ffmpeg_video->img_convert_ctx,
	    (const uint8_t *const *)ffmpeg_video->pFrame->data,
	    ffmpeg_video->pFrame->linesize, 0,
	    ffmpeg_video->pCodecCtx->height,
	    ffmpeg_video->pFrameBGRA->data, ffmpeg_video->pFrameBGRA->linesize);
  Imlib_Image image =
    imlib_create_image_using_copied_data(ffmpeg_video->pCodecCtx->width,
					 ffmpeg_video->pCodecCtx->height,
					 (uint32_t *)ffmpeg_video->buffer);
  return image;
}

/* aravis */

/* Aravis must have its own thread. Frames must be fetched from the
   camera at frame-rate. If we fall behind the camera will get
   stuck. */

void *aravis_thread(void *args) {
  struct aravis *aravis = (struct aravis *)args;
  while (aravis->capture_thread_running) {
    /* This will block until a frame is ready, up to 1s. The timeout
       is required because otherwise there is a race condition in
       shutting down the camera which might leave us waiting
       indefinitely */
    /* hardwired */
    ArvBuffer *buffer = arv_stream_timeout_pop_buffer(aravis->stream, 1000000);
    /* we timed out */
    if (buffer==NULL) continue;
    if (buffer->status==ARV_BUFFER_STATUS_SUCCESS) {
      /* swap out the current buffer with the new frame */
      pthread_mutex_lock(&aravis->buffer_lock);
      if (aravis->current_buffer) {
        arv_stream_push_buffer(aravis->stream, aravis->current_buffer);
      }
      aravis->current_buffer = buffer;
      pthread_mutex_unlock(&aravis->buffer_lock);
    }
    else {
      /* needs work: is buffer->status unsigned? */
      fprintf(stderr, "Bad frame with status %d in frame %u\n",
	      buffer->status, frame_number);
      arv_stream_push_buffer(aravis->stream, buffer);
    }
  }
  /* Return the current buffer. If we've been asked to stop the camera
     buffer has already returned its buffer and turned itself off, so
     we don't need to acquire a lock here. */
  if (aravis->current_buffer) {
    arv_stream_push_buffer(aravis->stream, aravis->current_buffer);
  }
  return NULL;
}

/* aravis_get_buffer and aravis_put_buffer fetch frames from the
   aravis thread. You must call aravis_put_buffer after
   aravis_get_buffer, every buffer must be put back before the camera
   is shut down. It is possible to request multiple buffers but this
   number should be kept reasonably small to not starve the aravis
   thread of buffer space */
ArvBuffer *aravis_get_buffer(struct aravis *aravis) {
  while (TRUE) {
    /* this will spin until a buffer is available */
    pthread_mutex_lock(&aravis->buffer_lock);
    if (aravis->current_buffer) {
      ArvBuffer *buffer = aravis->current_buffer;
      aravis->current_buffer = NULL;
      pthread_mutex_unlock(&aravis->buffer_lock);
      return buffer;
    }
    pthread_mutex_unlock(&aravis->buffer_lock);
    if (usleep(QUANTUM)) sft_error("Call to usleep failed");
  }
}

void aravis_put_buffer(struct aravis *aravis, ArvBuffer *buffer) {
  /* No need to lock because this uses g_async_queue internally which
     does its own locking. */
  arv_stream_push_buffer(aravis->stream, buffer);
}

struct aravis *aravis_setup(char *camera_id, double fps) {
  int x, y;
  g_type_init ();
  struct aravis *aravis = (struct aravis *)sft_malloc(sizeof(struct aravis));
  /* hardwired */
  arv_debug_enable("interface:1,device:1,stream:1,stream-thread:1,gvcp:1,gvsp:1,genicam:1,evaluator:1,misc:1");
  /* This takes a camera name, and without one will find the first
     available camera. Our Zebra is named:
     "Point Grey Research-12384103"
     Only arv-tool-0.2 reports the actual name. */
  aravis->camera = arv_camera_new(camera_id);
  if (!aravis->camera) sft_error("Couldn't find a GigE camera");
  aravis->device = arv_camera_get_device(aravis->camera);
  /* This sets up the basic parameters of the camera required to
     figure out the payload size. The same as running:

     arv-tool-0.2 control VideoMode=Mode4 OffsetX=4 OffsetY=54 Width=1216 Height=912 BinningHorizontal=2 BinningVertical=2

     The magic numbers above crop around the center of the camera and
     preserve the 640x480 aspect ratio. 2 by 2 binning means the
     original size is 1224x1024, this is the only binning for video
     mode 4. Video mode 6 corresponds to 4 by 4 binning.
  */
  /* hardwired */
  arv_device_set_string_feature_value(aravis->device, "VideoMode", "Mode4");
  arv_camera_set_binning(aravis->camera, 2, 2);
  arv_camera_set_region(aravis->camera, 4, 54, 1216, 912);
  arv_device_set_string_feature_value(aravis->device,
				      "PixelFormat",
				      "RGB8Packed");
  /* Even though we asked for a particular region the camera might
     give us a different height and width so we have to read that
     back otherwise the buffer size will be incorrect. */
  arv_camera_get_region
    (aravis->camera, &x, &y, &aravis->width, &aravis->height);
  aravis->stream = arv_camera_create_stream(aravis->camera, NULL, NULL);
  if (!aravis->stream) sft_error("The GigE camera is busy");
  int payload = arv_camera_get_payload(aravis->camera);
  if (payload==0) {
    sft_error("The camera got into a bad state and is going to give us empty frames");
  }
  /* Aravis needs enough buffers so that it will never fall behind the
     camera. Their examples use 50 buffers but everything seems ok
     with far fewer. */
  /* hardwired */
  for (unsigned int i = 0; i<50; i++) {
    arv_stream_push_buffer(aravis->stream, arv_buffer_new(payload, NULL));
  }
  /* other misc camera settings */
  arv_camera_set_acquisition_mode(aravis->camera,
                                  ARV_ACQUISITION_MODE_CONTINUOUS);
  /* really a boolean feature, but aravis doesn't support them
     explicitly. Note that max and min for boolean features are
     incorrect */
  /* hardwired */
  arv_device_set_integer_feature_value(aravis->device,
                                       "AcquisitionFrameRateEnabled",
                                       1);
  arv_device_set_string_feature_value(aravis->device,
                                      "AcquisitionFrameRateAuto",
                                      "Off");
  /* Set the frame rate of the camera slightly higher than our frame
     rate. The camera seems to treat the frame rate as an upper
     bound. */
  arv_device_set_float_feature_value(aravis->device,
                                     "AcquisitionFrameRate",
                                     fps+2);
  /* used for intermediate storage while converting to imlib */
  aravis->rgba_buffer =
    (unsigned char *)sft_malloc(aravis->height*aravis->width*4);
  if (pthread_mutex_init(&aravis->buffer_lock, NULL)!=0) {
    sft_error("Can't create aravis buffer lock");
  }
  return aravis;
}

void aravis_start(struct aravis *aravis) {
  arv_camera_start_acquisition(aravis->camera);
  aravis->capture_thread_running = 1;
  aravis->current_buffer = NULL;
  if (pthread_create(&aravis->capture_thread,
		     NULL,
		     aravis_thread,
		     (void *)aravis)!=0) {
    sft_error("Can't create aravis capture thread");
  }
}

void aravis_stop(struct aravis *aravis) {
  /* This is not a race condition because the capture thread will
     eventually time out and not wait for a final frame. It's better
     to stop acquisition before stopping the capture thread because if
     the buffer becomes full it might put the camera in a state that
     is difficult to recover from. */
  arv_camera_stop_acquisition(aravis->camera);
  aravis->capture_thread_running = 0;
  if (pthread_join(aravis->capture_thread, NULL)!=0) {
    sft_error("Can't join aravis capture thread");
  }
}

void aravis_cleanup(struct aravis *aravis) {
  pthread_mutex_destroy(&aravis->buffer_lock);
  g_object_unref(aravis->stream);
  g_object_unref(aravis->camera);
  /* No need to unref aravis->device. That's done as part of the
     cleanup of aravis->camera. */
  free(aravis->rgba_buffer);
  free(aravis);
}

Imlib_Image aravis_fetch_frame(struct aravis *aravis) {
  ArvBuffer *buffer = aravis_get_buffer(aravis);
  unsigned char *data = aravis->rgba_buffer;
  unsigned char *raw_data = buffer->data;
  unsigned int size = aravis->height*aravis->width;
  for (unsigned int i = 0; i<size; i++) {
    data[i*4+0] = raw_data[i*3+2];
    data[i*4+1] = raw_data[i*3+1];
    data[i*4+2] = raw_data[i*3+0];
    data[i*4+3] = 0;
  }
  Imlib_Image image =
    imlib_create_image_using_copied_data(aravis->width,
                                         aravis->height,
                                         (DATA32 *)aravis->rgba_buffer);
  aravis_put_buffer(aravis, buffer);
  return image;
}

/* V4L and other OpenCV supported cameras */

struct opencv_camera *opencv_camera_setup(void) {
  struct opencv_camera *opencv_camera =
    (struct opencv_camera *)sft_malloc(sizeof(struct opencv_camera));
  return opencv_camera;
}

void opencv_camera_start(struct opencv_camera *opencv_camera) {
  opencv_camera->capture = cvCreateCameraCapture(CV_CAP_ANY);
  if (!opencv_camera->capture) {
    sft_error("Can't open opencv camera for capture");
  }
  /* needs work: shouldn't be hardwired */
  /* needs work: This doesn't work on thakaa. */
  cvSetCaptureProperty(opencv_camera->capture, CV_CAP_PROP_FPS, 10);
  /* needs work: 800x600 doesn't work on thakaa, 640x480 does. */
  cvSetCaptureProperty
    (opencv_camera->capture, CV_CAP_PROP_FRAME_HEIGHT, HEIGHT);
  cvSetCaptureProperty(opencv_camera->capture, CV_CAP_PROP_FRAME_WIDTH, WIDTH);
  opencv_camera->outputBGRA =
    cvCreateImage(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 4);
}

void opencv_camera_stop(struct opencv_camera *opencv_camera) {
  cvReleaseCapture(&opencv_camera->capture);
}

void opencv_camera_cleanup(struct opencv_camera *opencv_camera) {
  free(opencv_camera);
}

Imlib_Image opencv_camera_fetch_frame(struct opencv_camera *opencv_camera) {
  /* cvQueryFrame returns a pointer to internal data */
  IplImage *frame = cvQueryFrame(opencv_camera->capture);
  cvCvtColor(frame, opencv_camera->outputBGRA, CV_RGB2BGRA);
  Imlib_Image image =
    imlib_create_image_using_copied_data
    (frame->width, frame->height,
     (unsigned int *)opencv_camera->outputBGRA->imageData);
  return image;
}

/* undistort */

Imlib_Image undistort_image(Imlib_Image image) {
  imlib_context_set_image(image);
  CvMat original_matrix =
    cvMat(imlib_image_get_height(),
	  imlib_image_get_width(),
	  CV_8UC4,
	  imlib_image_get_data_for_reading_only());
  unsigned char undistorted
    [imlib_image_get_height()][imlib_image_get_width()][4];
  CvMat undistorted_matrix = cvMat(imlib_image_get_height(),
				   imlib_image_get_width(),
				   CV_8UC4,
				   &undistorted[0][0][0]);
  CvMat calibration_matrix = cvMat(3, 3, CV_64FC1, &calibration[0]);
  CvMat distortion_matrix = cvMat(5, 1, CV_64FC1, &distortion[0]);
  cvUndistort2(&original_matrix,
               &undistorted_matrix,
               &calibration_matrix,
               &distortion_matrix,
	       /* needs work: is the next optional? */
               NULL);
  Imlib_Image undistorted_image =
    imlib_create_image_using_copied_data(imlib_image_get_width(),
                                         imlib_image_get_height(),
                                         (DATA32 *)&undistorted[0][0][0]);
  imlib_free_image_and_decache();
  return undistorted_image;
}

/* tasks */

void *camera_task(void *args) {
  struct task_args *task_args = (struct task_args *)args;
  unsigned int id = task_args->id;
  Display *display = XOpenDisplay("");
  int screen = DefaultScreen(display);
  struct ffmpeg_video *ffmpeg_video = NULL;
  struct aravis *aravis = NULL;
  struct opencv_camera *opencv_camera = NULL;
  Imlib_Image first_frame, playback_raw_frame = NULL;
  IplImage *output = cvCreateImage(cvSize(WIDTH, HEIGHT), IPL_DEPTH_8U, 3);
  CvVideoWriter *videowriter;
  /* magic so can display Imlib frames */
  imlib_context_disconnect_display();
  imlib_context_set_display(display);
  imlib_context_set_visual(XDefaultVisual(display, screen));
  imlib_context_set_colormap(XDefaultColormap(display, screen));
  imlib_context_set_drawable(display_pane);
  /* start frame source */
  frame_number = 0;
  switch (source) {
  case 0:
    aravis = aravis_setup(NULL, fps);
    aravis_start(aravis);
    /* This ignores the first few frames just to let the camera start up. One
       frame isn't enough for GigE cameras.
    */
    for (unsigned int i = 0; i<4; i++) {
      first_frame = aravis_fetch_frame(aravis);
      imlib_context_set_image(first_frame);
      /* need to check height<=HEIGHT&&width<=WIDTH */
      height = HEIGHT;
      width = WIDTH;
      imlib_free_image_and_decache();
    }
    if (usleep((int)floor(1000000.0/fps))) sft_error("usleep failed");
    break;
  case 1:
    opencv_camera = opencv_camera_setup();
    opencv_camera_start(opencv_camera);
    /* This ignores the first frame, just for camera, not for video, just to get
       the height and width to preallocate storage and let the camera start up.
    */
    first_frame = opencv_camera_fetch_frame(opencv_camera);
    imlib_context_set_image(first_frame);
    /* need to check height<=HEIGHT&&width<=WIDTH */
    height = HEIGHT;
    width = WIDTH;
    imlib_free_image_and_decache();
    break;
  default: sft_error("unknown source");
  }
  BARRIER("camera", "initialize");
  double last = current_time();
  /* per-frame loop */
  while (TRUE) {
    if (start_saving_frames) {
      videowriter =
	cvCreateVideoWriter((char *)&video[0],
			    // Some codecs, like X264, can't be used for
			    // writing.
			    CV_FOURCC('F', 'M', 'P', '4'),
			    fps,
			    cvSize(width, height),
			    1);
      if (videowriter==NULL) sft_error("Can't open output file");
      start_saving_frames = FALSE;
      saving_frames = TRUE;
      saving_frame_number = 0;
    }
    if (start_playback) {
      ffmpeg_video = ffmpeg_open_video((char *)&video[0]);
      if (ffmpeg_video==NULL) {
	sft_error("Couldn't open ffmpeg video %s", video);
      }
      /* need to check playback_height<=HEIGHT&&playback_width<=WIDTH */
      playback_raw_frame = NULL;
      start_playback = FALSE;
      playback_running = TRUE;
    }
    /* Check for signal from GUI. */
    pthread_mutex_lock(&halt_mutex);
    if (halt) running = FALSE;
    pthread_mutex_unlock(&halt_mutex);
    /* check if playback over */
    if (playback_running&&ffmpeg_video_finished(ffmpeg_video)) {
      playback_running = FALSE;
      ffmpeg_close_and_free_video(ffmpeg_video);
    }
    if (running) {
      /* get frame */
      if (raw_frame[CURRENT]!=NULL) {
	imlib_context_set_image(raw_frame[CURRENT]);
	imlib_free_image_and_decache();
        raw_frame[CURRENT] = NULL;
	cvReleaseImageHeader(&bgra[CURRENT]);
      }
      if (playback_raw_frame!=NULL) {
	imlib_context_set_image(playback_raw_frame);
	imlib_free_image_and_decache();
        playback_raw_frame = NULL;
      }
      /* read playback frame */
      if (playback_running) {
	playback_raw_frame = ffmpeg_get_frame_as_imlib(ffmpeg_video);
      }
      switch (source) {
      case 0:
	{
	  Imlib_Image big_frame = aravis_fetch_frame(aravis);
	  imlib_context_set_image(big_frame);
	  raw_frame[CURRENT] =
	    imlib_create_cropped_scaled_image(0, 0,
					      imlib_image_get_width(),
					      imlib_image_get_height(),
					      width, height);
	  imlib_free_image_and_decache();
	}
	break;
      case 1:
	raw_frame[CURRENT] = opencv_camera_fetch_frame(opencv_camera);
	break;
      default: sft_error("unknown source");
      }
      /* Convert to OpenCV format since it is thread safe. Some of this is
	 redundant with undistort. */
      bgra[CURRENT] =
	cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 4);
      imlib_context_set_image(raw_frame[CURRENT]);
      cvSetData(bgra[CURRENT],
		imlib_image_get_data_for_reading_only(),
		CV_AUTOSTEP);
      /* save frames */
      if (saving_frames) {
	/* Hopefully, this can keep up. */
	imlib_context_set_image(raw_frame[CURRENT]);
	unsigned char *data =
	  (unsigned char *)imlib_image_get_data_for_reading_only();
	CvScalar s;
	for (unsigned int y = 0; y<height; y++) {
	  for (unsigned int x = 0; x<width; x++) {
	    s.val[0] = data[4*(y*width+x)+0];
	    s.val[1] = data[4*(y*width+x)+1];
	    s.val[2] = data[4*(y*width+x)+2];
	    s.val[3] = data[4*(y*width+x)+3];
	    cvSet2D(output, y, x, s);
	  }
	}
	/* This segfaults if you try to write a .mov file. */
	cvWriteFrame(videowriter, output);
	saving_frame_number++;
	if (saving_frame_number==frames_to_save) {
	  cvReleaseVideoWriter(&videowriter);
	  saving_frames = FALSE;
	}
      }
    }
    /* wait until threads idle as late as possible before pipelining to
       give them as much time as possible to finish
       we used to report here which tasks were not done */
    BARRIER("camera", "before pipeline");
    /* increment frame number while threads are idle since they access it;
       we have a bug if it overflows */
    frame_number++;
    /* start up hog threads as soon as possible after pipelining to give them
       as much time as possible to finish */
    if (!running) break;
    BARRIER("camera", "after pipeline");
    /* displaying; this takes time so do it after starting up the threads;
       the display is one frame behind (because we need to wait for the hog
       threads) so can't display the first time around (which is when
       frame_number==2 since we just incremented it above */
    /* get next frame playback */
    if (playback_running) ffmpeg_next_frame(ffmpeg_video);
    /* flush */
    /* We flush to draw the imlib image from the previous iteration. */
    XFlush(display);
    /* The GUI will not draw anything with imlib after this and not use the
       imlib context after this. */
    /* The C code never draws anything directly to X. It only draws to an
       imlib image which then gets drawn to X here and only here. Now we
       finally draw the imlib context to X. Technically, we should flush
       again at this point. And if this was the case, the flush above would
       only be to send the event to run the GUI and this flush would be
       to update the screen. But we can just wait until the next iteration of
       the loop for the latter. */
    imlib_render_image_on_drawable(0, 0);
    if (playback_running) {
      imlib_context_set_image(playback_raw_frame);
      imlib_render_image_on_drawable(WIDTH, 0);
    }
    /* spin to sync to frame rate */
    double now = current_time();
    if (now-last>=1.0/fps) {
      fprintf(stderr,
	      "camera can't keep up in frame %u, overused: %lf\n",
	      frame_number, fps*(now-last)-1.0);
    }
    while (TRUE) {
      double now = current_time();
      if (now-last>=1.0/fps) {
	last = now;
	break;
      }
      if (usleep(QUANTUM)) sft_error("Call to usleep failed");
    }
  }
  BARRIER("camera", "finalize");
  /* free resources */
  switch (source) {
  case 0:
    aravis_stop(aravis);
    aravis_cleanup(aravis);
    break;
  case 1:
    opencv_camera_stop(opencv_camera);
    opencv_camera_cleanup(opencv_camera);
    break;
  default: sft_error("unknown source");
  }
  cvReleaseImage(&output);
  for (unsigned int pipeline = 0; pipeline<PIPELINE; pipeline++) {
    if (raw_frame[pipeline]!=NULL) {
      imlib_context_set_image(raw_frame[pipeline]);
      imlib_free_image_and_decache();
      raw_frame[pipeline] = NULL;
      cvReleaseImageHeader(&bgra[pipeline]);
    }
  }
  if (playback_raw_frame!=NULL) {
    imlib_context_set_image(playback_raw_frame);
    imlib_free_image_and_decache();
    playback_raw_frame = NULL;
  }
  if (playback_running) {
    playback_running = FALSE;
    ffmpeg_close_and_free_video(ffmpeg_video);
  }
  XClearWindow(display, display_pane);
  XFlush(display);
  return NULL;
}

void start_threads(void) {
  if (running) sft_error("threads already running");
  if (pthread_barrier_init(&barrier, NULL, threads)) {
    sft_error("Can't create a barrier");
  }
  running = TRUE;
  for (unsigned int id = 0; id<threads; id++) {
    if (task[id]!=&camera_task) {
      task_args[id].id = id;
    }
  }
  pthread_attr_t attributes;
  pthread_attr_init(&attributes);
  pthread_attr_setstacksize(&attributes, 10*1024*1024); /* hardwired 10MB */
  for (unsigned int id = 0; id<threads; id++) {
    if (pthread_create(&thread[id], &attributes, task[id],
		       (void *)&task_args[id])) {
      sft_error("Can't start thread %u", id);
    }
  }
  pthread_attr_destroy(&attributes);
}

void stop_threads(void) {
  /* needs work: Technically, can't access running here without a mutex. */
  if (running&&!halt) {
    pthread_mutex_lock(&halt_mutex);
    halt = TRUE;
    pthread_mutex_unlock(&halt_mutex);
    for (unsigned int id = 0; id<threads; id++) {
      if (pthread_join(thread[id], NULL)!=0) {
	sft_error("Can't join thread %u", id);
      }
    }
    pthread_barrier_destroy(&barrier);
    halt = FALSE;
  }
}

void playback(char *pathname) {
  if (!playback_running) {
    if (strlen(pathname)<MAX_VIDEO) {
      strcpy((char *)&video[0], pathname);
      start_playback = TRUE;
    }
  }
}

void write_saved_frames(char *pathname) {
  if (!saving_frames&&!start_saving_frames) {
    if (strlen(pathname)<MAX_VIDEO) {
      strcpy((char *)&video[0], pathname);
      start_saving_frames = TRUE;
    }
  }
}

/* Tam V'Nishlam Shevah L'El Borei Olam */
