/* 
 * V4l2 Camera Motor Control Script
 * (c) 2008, Christopher "ScribbleJ" Jansen
 *
 * This short script will allow you a simple way to control the 
 * motor of a V4l2 camera, such as the Logitech Orbit MP, which 
 * is the camera I own and tested this script on. 
 *
 * You are free to use, modify, and distribute this script however
 * you want, but if you do I'd really love to hear from you about it.
 * Just shoot me an email that says "thanks" if nothing else!
 *
 * My email address is: scribblej@scribblej.com
 *
 */
#include <stdio.h>
#include <stdlib.h>
#include <fcntl.h>
#include <string.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <linux/videodev2.h>

/*
 * I don't know if the three values below are likely to be the same on other
 * cameras, other systems, other versions, other anything, actually.
 *
 * I derived them by running this script with the -q option.  If they
 * don't work for you, you might try the same.
 */
#define PAN (V4L2_CID_PRIVATE_BASE + 7) 
#define TILT (V4L2_CID_PRIVATE_BASE + 8)
#define RESET (V4L2_CID_PRIVATE_BASE + 9)


static char * usage =
  "V4L2 Camera Motor Control Script by ScribbleJ\n"
  "Usage: \n"
  "\n"
  " -u <num> = UP num units\n"
  " -d <num> = DOWN num units\n"
  " -l <num> = LEFT num units\n"
  " -r <num> = RIGHT num units\n"
  " -x = Reset cam position (causes motor grind)\n"
  " note: Good options for <num> are 10 for a small move and 100 for a large move.\n"
  "\n"
  "Other Options:\n"
  " -v <device> = Set video device\n"
  " -q = Display camera controls\n";
 

/* Sets the value of a V4l2 control. */
int setControl(int fd, int c_id, int value)
{
  struct v4l2_control c;

  memset(&c, 0, sizeof(c));
  c.id = c_id;
  c.value = value;

  if(-1 == ioctl(fd, VIDIOC_S_CTRL, &c))
  {
    fprintf(stderr, "Couldn't set control.\n");
    exit(0);
  }

  return 1;
}

/*
 * Prints (to stdout) a list of all the controls in the "Private"
 * range.  This does not print ALL controls, but the V4l2 docs
 * include a great example of how to do that if it's your thing.
 */
int enumControls(int fd)
{
  struct v4l2_queryctrl qc;

  for(qc.id = V4L2_CID_PRIVATE_BASE;; qc.id++)
  {
    if(0 == ioctl(fd, VIDIOC_QUERYCTRL, &qc))
    {
      if(qc.flags & V4L2_CTRL_FLAG_DISABLED)
        continue;


      printf("%d %s min: %d max: %d default: %d", qc.id - V4L2_CID_PRIVATE_BASE, qc.name, qc.minimum, qc.maximum, qc.default_value);

      if (qc.type == V4L2_CTRL_TYPE_MENU)
        printf(" <--This is a menu\n");
      else 
        printf("\n");
    }
    else
    {
      return(0);
    }
  }
}


int main(int argc, char * argv[])
{
  int fd;
  char *device = "/dev/video0";

  int move_type = 0;
  int move_amount = 0;
  int query_controls = 0;

  int c;
  while((c=getopt(argc, argv, "u:d:l:r:v:xq")) != -1)
  {
    switch (c)
    {
      case 'q':
        query_controls = 1;
        break;

      case 'v':
        device = optarg;
        break;

      case 'l':
        sscanf(optarg, "%d", &move_amount);
        move_type = 1;
        break;

      case 'r':
        sscanf(optarg, "%d", &move_amount);
        move_type = 1;
        move_amount *= -1;
        break;

     case 'd':
        sscanf(optarg, "%d", &move_amount);
        move_type = 2;
        break;

     case 'u':
        sscanf(optarg, "%d", &move_amount);
        move_type = 2;
        move_amount *= -1;
        break;

     case 'x':
        move_type = 3;
        break;

     default:
       fprintf(stderr, "%s", usage);
       exit(1);
    }
  }

  move_amount *= 10;

  // printf("Move amount: %d Move type: %d Device: %s\n", move_amount, move_type, device);

  if((fd = open(device, O_RDWR)) == -1)
  {
    fprintf(stderr, "Error opening device: %s\n", device);
  }

  if(query_controls)
  {
    printf("\n");
    enumControls(fd);
    printf("\n");
  }
  else 
  switch(move_type)
  {
    case 1:
      setControl(fd, PAN, move_amount);
      break;
    case 2:
      setControl(fd, TILT, move_amount);
      break;
    case 3:
      setControl(fd, RESET, 3);
      break;
    default:
        fprintf(stderr, "%s", usage); 
  }
  exit(0);
}