VideoSource_V4L.cpp

/*
This file is part of the BIAS library (Basic ImageAlgorithmS).

Copyright (C) 2003-2009    (see file CONTACT for details)
Multimediale Systeme der Informationsverarbeitung
Institut fuer Informatik
Christian-Albrechts-Universitaet Kiel


BIAS is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation; either version 2.1 of the License, or
(at your option) any later version.

BIAS is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with BIAS; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include <sys/types.h>
#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
//#include <linux/videodev.h>
//#include <libv4lconvert.h>
#include <errno.h>



#include "VideoSource_V4L.hh"
#include "VideoSourceCapabilities.hh"
#include <Base/Common/FileHandling.hh>
#include <Base/Debug/Exception.hh>
#include <Image/Camera.hh>
#include <Base/Image/ImageIO.hh>

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

using namespace BIAS;
using namespace std;



VideoSource_V4L::
VideoSource_V4L()
  : VideoSource()
{

  ColorMode_ = ImageBase::CM_RGB;
  DefaultDevice_ = "/dev/video0";
  ActiveFrame_ = 0;
  SetDebugLevel(0);
  // try PAL as default
  Width_=768;
  Height_=576;
  ColorChannels_=3;
  DeviceChannel_=-1;
  newFrame_=false;
  if (pthread_mutex_init(&ActiveFrameMutex_, NULL)!=0)
    BEXCEPTION("error initializing mutex");
  // may be some time in the future.
  // We could ask the V4L API if the device is controllable
  IsControllable_ = false;
  Frame_.resize(2);
  ReadFirstFrame_ = false;
  ControlBrightness_.id = 0;
  ControlContrast_.id = 0;  
  ControlGain_.id = 0;  
  ControlAutoGain_.id = 0;
  ControlShutter_.id = 0;
  //  SetDebugLevel(D_V4L);
  fd = -1;
  isJPEG_ = false;
  BIASDOUT(D_V4L,"Contructor finished");
  minBrightness_ =0; //additive offset in pixel values
  maxBrightness_ = 100;
}


VideoSource_V4L::
~VideoSource_V4L()
{
  if (fd>=0) close(fd);
 BIASDOUT(D_V4L,"Destructor finished");

}



int VideoSource_V4L::
GetAllDevices(std::vector<std::string> &devices)
{
    BIASDOUT(D_V4L,"GetAllDevices()");
    vector<string> devs;
    string IdentifierBak = Identifier_;
    ModelNameToDevice_.clear();
    FileHandling::GetDirectoryContent("/dev","video","",devs,true);
    for (unsigned int i=0;i<devs.size(); i++){
        struct v4l2_capability cap;
        string CoolName;
        if ((fd = open(devs[i].c_str(), O_RDWR))>0) {
            if (xioctl_(fd, (int)VIDIOC_QUERYCAP, &cap)>=0) {
                Identifier_ = (char*)cap.card;
                FixIdentifierString_( );
                ModelNameToDevice_[Identifier_] = devs[i];
                devices.push_back(Identifier_);

            }else
                BIASERR("Enumerating devices, query capabilities failed for "<<devs[i]<<": "<<strerror(errno));
            close(fd);
            fd = -1;
        } else
            BIASERR("Enumerating devices, open() failed for "<<devs[i]<<": "<<strerror(errno));
    } //for

    Identifier_ = IdentifierBak;
    return devices.size();
}



int VideoSource_V4L::
GetCapabilities(VideoSourceCapabilities &caps)
{
  return GetCapabilities(DefaultDevice_,caps);
}


int VideoSource_V4L::
GetCapabilities(const char *device, VideoSourceCapabilities &caps)
{
    deviceChannelNames_.clear();
  BIASDOUT(D_V4L,string("GetCapabilities() for device ")+device);
  caps.Reset();
  string usedev;
  if (device[0] != '/') {
    usedev = ModelNameToDevice_[device];
    if (usedev =="") return -2;
  }
  else usedev =device;
  if ((fd = open(usedev.c_str(), O_RDWR))<0) {
    fd = -1;
    BEXCEPTION("Failed to open device: "<<usedev<<" "<<strerror(errno));
  }

  if (xioctl_(fd,(int)VIDIOC_QUERYCAP,&capability2)<0) {
    close(fd);
    fd = -1;
    BEXCEPTION("Failed to query capabilities: "<<strerror(errno));
  }
  caps.SetName(string((char*)capability2.card));

#ifdef BIAS_DEBUG
  cout << "driver:       " << capability2.driver     << "\n";
  cout << "Bus:          " << capability2.bus_info   <<endl;
  cout<<  "Version:      " << capability2.version    <<endl;
  if (capability2.capabilities & V4L2_CAP_VIDEO_CAPTURE)
    cout<<"The device supports the video capture interface."<<endl;
  if (capability2.capabilities & V4L2_CAP_VIDEO_OUTPUT)
    cout<<"The device supports the video output interface."<<endl;
  if (capability2.capabilities &V4L2_CAP_VIDEO_OVERLAY)
    cout<<"The device supports the video overlay interface. Overlay "
    "typically stores captured images directly in the video memory "
    "of a graphics card, with support for clipping."<<endl;
  if (capability2.capabilities & V4L2_CAP_VBI_CAPTURE)
    cout<<"The device supports the VBI capture interface"<<endl;
  if (capability2.capabilities & V4L2_CAP_TUNER)
    cout<<"The device has a tuner of some form"<<endl;
  if (capability2.capabilities & V4L2_CAP_AUDIO)
    cout<<"The device has audio inputs or outputs."<<endl;
  if (capability2.capabilities &V4L2_CAP_READWRITE)
    cout<<"The device supports the read() and/or write() I/O methods."<<endl;
  if (capability2.capabilities & V4L2_CAP_ASYNCIO)
    cout<<"The device supports the asynchronous I/O methods."<<endl;
  if (capability2.capabilities & V4L2_CAP_STREAMING)
    cout<<"The device supports the streaming I/O method."<<endl;
#endif
  // if CAPTURE device print info about all inputs
  int index=0, current=-1;
  struct v4l2_input input;
  memset (&input, 0, sizeof (input));
  input.index = index;
  if (xioctl_(fd,(int)VIDIOC_G_INPUT, &current)!=0){
    perror("couldn't get current active input number");
  }

  while (xioctl_(fd, (int)VIDIOC_ENUMINPUT, &input)==0) {
    VideoSourceCapabilities::InputDescriptor InpDesc;
    InpDesc.number = index;
    InpDesc.name = (char*)input.name;
    if (input.type == V4L2_INPUT_TYPE_CAMERA) 
      InpDesc.type = VideoSourceCapabilities::TypeCamera;

    if (input.type == V4L2_INPUT_TYPE_TUNER) 
      InpDesc.type = VideoSourceCapabilities::TypeTuner;

     //cout<<"Supported standards:";
    //     if (input.std & V4L2_STD_PAL ) cout<<"PAL ";
    //     if (input.std & V4L2_STD_NTSC ) cout<<"NTSC ";
    //     if (input.std & V4L2_STD_SECAM ) cout<<"SECAM ";
     //cout<<endl;
    if (index==current) {
      InpDesc.Active = true;
      if  (input.status & V4L2_IN_ST_NO_POWER) InpDesc.NoPower = true;
      else InpDesc.NoPower = false;
      if  (input.status & V4L2_IN_ST_NO_SIGNAL) InpDesc.NoSignal = true;
      else InpDesc.NoSignal = false;
      if  (input.status & V4L2_IN_ST_NO_COLOR)  InpDesc.NoColor = true;
      InpDesc.NoColor = false;
    }
    else       InpDesc.Active = false;

    //back up channel names
    deviceChannelNames_.push_back(InpDesc.name);

    caps.AddInput(InpDesc);
    index++;
    input.index = index;
    usleep(1000); //wait 1ms
  }
  if (index==0) {
    cerr<<"No Input found. Error:";
    if (errno==EBUSY) cout<<"EBUSY"<<endl;
    if (errno==EINVAL) cout<<"EINVAL"<<endl;
    else cout<<errno<<endl;
  }

 
  struct v4l2_fmtdesc fmtdesc;
  fmtdesc.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  fmtdesc.index = 0;

  while (xioctl_(fd,(int)VIDIOC_ENUM_FMT,&fmtdesc) ==0 ) {
    ImageBase::EColorModel cm;
    switch(fmtdesc.pixelformat) {
    case v4l2_fourcc('B', 'G', 'R', '3'): cm = ImageBase::CM_BGR; break;
    case v4l2_fourcc('R', 'G', 'B', '3'): cm = ImageBase::CM_RGB; break;
    case v4l2_fourcc('Y', 'U', 'Y', 'V'): cm = ImageBase::CM_YUYV422; break;
    case v4l2_fourcc('U', 'Y', 'V', 'Y'): cm = ImageBase::CM_UYVY422; break;
    case v4l2_fourcc('G', 'B', 'R', 'G'): cm = ImageBase::CM_Bayer_GBRG; break;
    case v4l2_fourcc('B', 'A', '8', '1'): cm = ImageBase::CM_Bayer_BGGR; break;
    case v4l2_fourcc('G', 'R', 'E', 'Y'): cm = ImageBase::CM_Grey; break;
    //todo add switches for other color models:
    //HI24 RGBO RGBQ RGBP RGBR BGR4 RGB4 422P YU12 YV12 411P YUV9 YVU9
    default: {
        char tmp[10];
        memcpy(tmp, (char*)&fmtdesc.pixelformat,4);
        tmp[4]=0;
        //cout <<"unsupported pixelformat: "<<tmp<<endl;
        cm = ImageBase::CM_invalid;
        }
    }//end switch

    if (cm != ImageBase::CM_invalid) {
        //cout<<"FOUND VALID ColorMode:"<<fmtdesc.pixelformat<<endl;
        struct  v4l2_frmsizeenum frmsizeenum;
        frmsizeenum.index = 0;
        frmsizeenum.pixel_format = fmtdesc.pixelformat;
        bool interlace = true;
        switch(fmtdesc.pixelformat)
        {
        case V4L2_FIELD_NONE:
            interlaceMode_ = V4L2_FIELD_NONE;
            interlace=false;
            break;
        case V4L2_FIELD_ANY:
        case V4L2_FIELD_TOP :
        case V4L2_FIELD_BOTTOM :
        case V4L2_FIELD_INTERLACED:
        case V4L2_FIELD_SEQ_TB:
        case V4L2_FIELD_SEQ_BT:
        case V4L2_FIELD_ALTERNATE:
        case V4L2_FIELD_INTERLACED_TB :
        case V4L2_FIELD_INTERLACED_BT:
            interlaceMode_ = V4L2_FIELD_INTERLACED;
            interlace = true;
            break;
        default:
            interlace = false;
            interlaceMode_ = V4L2_FIELD_NONE;
            //cout<<"Unknown interlace type:"<<fmtdesc.pixelformat<<endl;
            break;
        }

        while (-1 != xioctl_(fd,(int)VIDIOC_ENUM_FRAMESIZES,&frmsizeenum)) {
            cout <<" Framesize type: "<<frmsizeenum.type<<endl;
            switch (frmsizeenum.type)
            {
            case V4L2_FRMSIZE_TYPE_DISCRETE: {
                struct v4l2_frmivalenum frmivalenum;
                frmivalenum.pixel_format = fmtdesc.pixelformat;
                frmivalenum.width = frmsizeenum.discrete.width;
                frmivalenum.height = frmsizeenum.discrete.height;
                frmivalenum.index = 0;

                float fps;
                while (xioctl_(fd,(int)VIDIOC_ENUM_FRAMEINTERVALS, &frmivalenum) ==0) {
                    switch (frmivalenum.type) {
                    case    V4L2_FRMIVAL_TYPE_DISCRETE:
                        fps = (float)frmivalenum.discrete.denominator/(float)frmivalenum.discrete.numerator;
                        caps.AddMode(frmsizeenum.discrete.width,frmsizeenum.discrete.height,fps,cm,interlace);
                        cout<<"Adding Mode:"<<frmsizeenum.discrete.width<<","<<frmsizeenum.discrete.height<<endl;
                        break;
                    case    V4L2_FRMIVAL_TYPE_STEPWISE:
                        break;
                    case    V4L2_FRMIVAL_TYPE_CONTINUOUS:
                        break;
                    }
                    frmivalenum.index ++;
                }
            }
            break;
            case V4L2_FRMSIZE_TYPE_CONTINUOUS:
                BIASERR("VideoSourceCapabilities can not handle continuous frame sizes");
                break;
            case V4L2_FRMSIZE_TYPE_STEPWISE:
                BIASERR("VideoSourceCapabilities can not handle continuous frame sizes");
                break;
            }
            cout<<"we found :"<<frmsizeenum.index<<" valid configurations."<<endl;
            frmsizeenum.index++;

        } // while () for all frmsizees
    } // if (cm != ImageBase::CM_invalid)
    else{
        ;//cout<<"Invalid ColorMode:"<<fmtdesc.pixelformat<<endl;
    }
    fmtdesc.index++;
  } // while () for all pixelformats
  close(fd);
  fd = -1;
  return 0;
}

void VideoSource_V4L::
GetDeviceChannelNames(std::vector<std::string> &names){
    names = deviceChannelNames_;
}

int VideoSource_V4L::
OpenDevice()
{
  return OpenDevice(DefaultDevice_);
}


int VideoSource_V4L::
OpenDevice(const char *device)
{
  string usedev;
  if (device[0] != '/') {
    if (ModelNameToDevice_.empty()) {
      vector<string> tmp;
      GetAllDevices(tmp);
      device = tmp[0].c_str();
    }
    cout<<"Device in OpenDevice:"<<device<<endl;
    usedev = ModelNameToDevice_[device];
    if (usedev =="") BEXCEPTION("Unknown device "<<device);
    BIASDOUT(D_V4L,string("OpenDevice(")+device+") will use "+usedev);
  }
  else {
    usedev =device;
    BIASDOUT(D_V4L,string("OpenDevice() will use ")+usedev);
  }
#ifdef BIAS_DEBUG
  cout<<"Opening device :"<<usedev<<endl;
#endif
  if ((fd = open(usedev.c_str(), O_RDWR))<0){
    BIASERR("Failed to open device: "<<usedev<<" "<<strerror(errno));
    return -1;
  }

  if (xioctl_(fd,(int)VIDIOC_QUERYCAP,&capability2)<0){
    BIASERR("Failed query capabilities: "<<strerror(errno));
    return -1;
  }

  // at first try to select the input/channel where a signal is
  // only if none ist set by the user

  int index =0;
#ifdef BIAS_DEBUG
  cout<<"V4L device channel:"<<DeviceChannel_<<endl;
#endif
  if (DeviceChannel_>=0) {
    index = DeviceChannel_;
  }

  struct v4l2_input input;
  int res=0;
  res=ioctl(fd, VIDIOC_S_INPUT, &index);
  if (res!=0){
    perror("VIDIOC_S_INPUT: couldn't set active input:");
  }
  bool found =false;
  while (!found && res==0) {
    // check wether there is a signal and color
    memset (&input, 0, sizeof (input));
    input.index = index;
    usleep(50);
    if (ioctl(fd, VIDIOC_ENUMINPUT, &input)==0) {
        if (input.status==0) {
            found=true; // input with a valid signal found
            // cout<<"valid signal on input channel:"<<index<<" found."<<endl;
        }
    }
    if (!found) {
        usleep(50);
        index++;
        res=xioctl_(fd, (int)VIDIOC_S_INPUT, &index);
        if (res!=0)
            cerr<<"VIDIOC_S_INPUT: couldn't set active input:"<<index<<endl;
    }
  }
  if (!found) {
    close(fd);
    fd = -1;
    BIASERR("No valid input signal found on all "<<index+1<<" channels");
    return -1;
  }

  res = InitDevice_();
  if (res!=0) return res;

  res = InitMMap_();
  if (res!=0) return res;

  CompleteInitialized_ = true;

#ifdef BIAS_HAVE_LIBJPEG
 if (isJPEG_) {
   if (Decompressor_.Init() < 0) {
     BIASERR("Could not initialize JPEG-compression!!!");
     return -1;
   }
   else cout<<"Initialized jpeg decompressor."<<endl;
 }
#else //BIAS_HAVE_LIBJPEG
 if (isJPEG_) {
   BIASERR("Compression mode not available without LIBJPEG!");
 }
#endif //BIAS_HAVE_LIBJPEG
  
  InitImage(Frame_[0]);
  InitImage(Frame_[1]);

  //ImageIO::Save("Frame.mip", Frame_[0]);

  return 0;
}

int VideoSource_V4L::
OpenDevice(int device)
{
  char dev[256];
  sprintf(dev, "/dev/video%d", device);
  return OpenDevice(dev);
}

int VideoSource_V4L::
CloseDevice()
{
  BIASDOUT(D_V4L,"Closing device");
  if (Active_) PostGrab();

  Grabbing_ = false;

  CompleteInitialized_ = false;

  int res = 0;
  res = UninitMMap_();
  if (res!=0) {
    BIASERR("UninitMMap_() Failed ");
    return res;
  }

  if (-1 == close(fd)){
    cerr << "error calling close()\n";
    perror("close");
  }
  fd = -1;
  return 0;

}



int VideoSource_V4L::
GrabSingle(Camera<unsigned char> &image)
{
    //  BIASDOUT(D_V4L,"VideoSource_V4L::GrabSingle()");
    assert(!image.IsEmpty());
    if (!Active_) BEXCEPTION("VideoSource Object not active, use PreGrab()");
    if (Grabbing_) BEXCEPTION("VideoSource Object is already grabbing()");

    Grabbing_ = true;
    //check Active multiple times to be really sure
    if (Active_ ) { // Has PreGrab() spawned a grab thread?
        // Has that thread delivered an image ? If not, wait;
        while (!ReadFirstFrame_) usleep(1000);
    }

    if (Active_ ) {
        if (WaitForNew_) while (!newFrame_) usleep(500);
    }

    pthread_mutex_lock(&ActiveFrameMutex_);
    if (Active_ ) {
        // locking not necessary because more than 2 buffers are used fro grabbing
        gettimeofday(&GrabTime_,NULL);
        image.SetTime(GrabTime_.tv_sec,GrabTime_.tv_usec);
        newFrame_ = false;
        //cout << "GrabSingle, returning Frame_["<<(int)(ActiveFrame_)<<"] ... "<<flush;

#ifdef BIAS_DEBUG
        if (Frame_[ActiveFrame_].GetSizeByte() != image.GetSizeByte()) {
            BIASERR("intern buffer has size: "<<Frame_[ActiveFrame_].GetSizeByte() 
                    <<"target buffer has size: "<<image.GetSizeByte());
        }
#endif
        if(Frame_[ActiveFrame_].GetImageData()==NULL){
            Frame_[ActiveFrame_].Init(Frame_[ActiveFrame_].GetWidth(),
                    Frame_[ActiveFrame_].GetHeight(),Frame_[ActiveFrame_].GetChannelCount());
        }
        image.CopyIn_NoInit((unsigned char*)Frame_[ActiveFrame_].GetImageData());
        //cout << " done\n";
    } else { // no grab thread has to do the hard work
        buffer_.index=0; // always use first buffer, if not in continous mode
        if (xioctl_(fd, (int)VIDIOC_QBUF, &buffer_)!=0) {
            perror("ioctl: freeing the buffer memory\n");
            Grabbing_ = false;
            pthread_mutex_unlock(&ActiveFrameMutex_);
            return -3;
        }
        errno=0;
        // wait for a new frame
        while ( (errno!=EINVAL) &&
                (xioctl_(fd, (int)VIDIOC_DQBUF, &buffer_)!=0) ) {
            if (errno==EINVAL) {
                Grabbing_ = false;
                //BEXCEPTION("Error while waiting for a new frame: "<<strerror(errno));
                BIASERR("Error while waiting for a new frame: "<<strerror(errno));
                pthread_mutex_unlock(&ActiveFrameMutex_);
                return -4;
            }
        }
        gettimeofday(&GrabTime_,NULL);
        image.SetTime(GrabTime_.tv_sec,GrabTime_.tv_usec);
        image.CopyIn_NoInit( (unsigned char*)Buffers_[0].start);
    }
    pthread_mutex_unlock(&ActiveFrameMutex_);

    Grabbing_ = false;

    image.InvalidateUID();
    image.SetUID(UUID::GenerateUUID());

    //   BIASDOUT(D_V4L,"leaving GrabSingle()");

  return 0;
}

int VideoSource_V4L::
PreGrab()
{
  BIASDOUT(D_V4L,"PreGrab()");
  Active_ = true;
  int res = StartCapturing_();
  if (res!=0){// BEXCEPTION("StartCapturing_() failed");
     BIASERR("StartCapturing_() failed");
     return -1;
  }
  pthread_attr_t attributes;
  pthread_attr_init(&attributes);
  int detachstate;
  pthread_attr_getdetachstate(&attributes,&detachstate);
  // cout <<"Thread detach state: "<<detachstate<<endl<<" joinable would be: "
  //      <<PTHREAD_CREATE_JOINABLE<<endl;
  pthread_attr_setdetachstate(&attributes,PTHREAD_CREATE_JOINABLE);
  //pthread_mutex_init(&ActiveFrameMutex_, NULL) ||
  return pthread_create(&ContinousThread_, &attributes, GrabContThread_ , this);
}


int VideoSource_V4L::
PostGrab()
{
  BIASDOUT(D_V4L,"PostGrab()");
  int res = 0;
  if (Active_) {
    Active_ = false;
    usleep(1000);
    if ( (res = pthread_join(ContinousThread_, NULL)) != 0){
      BIASERR("VideoSource_V4L::PostGrab()  pthread_join unsucessfull: "
          << strerror(errno));
    }
  }
  pthread_mutex_unlock(&ActiveFrameMutex_);
  res = StopCapturing_();
  if (res!=0) BIASERR("StopCapturing_() failed");
  return 0;
}



void VideoSource_V4L::
FixIdentifierString_()
{
    if (Identifier_.find("UVC Camera") != string::npos) {
        cout <<"This is an UVC-Camera on USB, Identifier: "<<Identifier_<<endl;
        int StartOfId = Identifier_.find('(')+1;
        string USBID = Identifier_.substr(StartOfId,9);
        /// try to extract vendor/product string from
        /// system's map file
        /// mapfile's syntax:
        /// vendorID  vendor string
        /// \t productID  product
        string mapfile;
        vector<string> possibleMapFiles;
        /// to be extended as necessary
        possibleMapFiles.push_back("/var/lib/misc/usb.ids"); /// ubuntu jaunty
        for ( unsigned f=0; f<possibleMapFiles.size(); f++ ) {
            ifstream ifs(possibleMapFiles[f].c_str());
            if (ifs) {
                mapfile = possibleMapFiles[f];
                ifs.close();
                break;
            }
        }
        if ( mapfile == "" ) return;

        string vendorID = USBID.substr(0,4);
        string productID = USBID.substr(5,4);
        string identifier;

        ifstream ifs(mapfile.c_str());
        if (!ifs) cerr << "Cannot open file: " << mapfile << endl;
        else {

            /// first: find vendor
            while ( !ifs.eof() ) {
                string tmp;
                getline(ifs, tmp);
                if ( tmp.substr(0,4) == vendorID ) {
                    int vendorStart = tmp.find_first_not_of(" ", 4);
                    identifier = tmp.substr(vendorStart);
                    //cout << "found vendor " << identifier << endl;
                    /// found vendor, stop
                    break;
                }
            } // end while vendor

            /// second: find product
            while ( !ifs.eof() ) {
                string tmp;
                getline(ifs, tmp);
                int productIDStart = tmp.find_first_not_of("\t ");
                if ( productIDStart == 0 ) {
                    /// reached next vendor, i.e. no product found
                    break;
                }
                if ( tmp.substr(productIDStart, 4) == productID ) {
                    tmp = tmp.substr(productIDStart);
                    int productIDEnd = tmp.find_first_of(" ");
                    int productStart = tmp.find_first_not_of(" ", productIDEnd);
                    if ( identifier != "" ) identifier += " ";
                    identifier += tmp.substr(productStart);
                    //cout << " found product " << tmp.substr(productStart) << endl;
                    /// found product, stop
                    break;
                }
            } // end while product

            ifs.close();

        }

        if ( identifier != "" ) {
            Identifier_ = identifier;
            cout <<"  New Identifiers_ "<<Identifier_<<endl;
        }
    } // if("UVC camera")
    else {
      // fkellner 15.2.11 why is this converted to utf8?
        // Identifier_ = IsoLatin1ToUtf8(Identifier_);
    }
}


void *VideoSource_V4L::
GrabContThread_(void * arg)
{
  VideoSource_V4L *obj = (VideoSource_V4L *)arg;

  while (obj->Active_){

    fd_set fds;
    struct timeval tv;
    int r;

    FD_ZERO (&fds);
    FD_SET (obj->fd, &fds);

    /* Timeout. */
    tv.tv_sec = 2;
    tv.tv_usec = 0;

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

    if (-1 == r) {
      if (EINTR == errno){
        continue;
      }
      cerr << "select\n";
      perror("select");
      return NULL;
    }

    if (0 == r) {
      cerr << "select timeout\n";
      continue;
      //return NULL;
    }

    if ( obj->ReadFrame_() != 0 ) {
      cerr << "lost camera\n";
      obj->Active_ = false;
    }

    /* EAGAIN - continue select loop. */
  }
#ifdef BIAS_DEBUG
  cout <<"finished grab thread"<<endl;
#endif
  pthread_exit(NULL);
}


int VideoSource_V4L::
InitMMap_()
{
  //  cout << "InitMMap_()\n";
  struct v4l2_requestbuffers req;
  CLEAR(req);

  req.count               = 4;
  req.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  req.memory              = V4L2_MEMORY_MMAP;

  if (-1 == xioctl_(fd, (int)VIDIOC_REQBUFS, &req)) {
    if (EINVAL == errno) 
      BEXCEPTION("Device does not support memory mapping:"<<strerror(errno))
    else 
      BEXCEPTION("Failed to request buffers: "<<strerror(errno));
  }

  if (req.count < 2) 
    BEXCEPTION("Insufficient buffer memory, need at least 2 buffers"<<strerror(errno));

  Buffers_.resize(req.count);

  for (unsigned i = 0; i < req.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 == xioctl_(fd, (int)VIDIOC_QUERYBUF, &buf))
      BEXCEPTION("Failed to query buffer: "<<strerror(errno));
     
    Buffers_[i].length = buf.length;
    Buffers_[i].start =
      mmap (NULL /* start anywhere */,
            buf.length,
            PROT_READ | PROT_WRITE /* required */,
            MAP_SHARED /* recommended */,
            fd, buf.m.offset);

    if (MAP_FAILED == Buffers_[i].start)
      BEXCEPTION("Failed to map memory: "<<strerror(errno));
  }
  return 0;
}


int VideoSource_V4L::
xioctl_(int fd, int request, void *arg)
{
  int r;
  do r = ioctl(fd, request, arg);
  while (-1 == r && EINTR == errno);

  return r;
}


int VideoSource_V4L::
StartCapturing_()
{
  enum v4l2_buf_type type;

  const unsigned num_buf = Buffers_.size();
  for (unsigned i = 0; i < num_buf; ++i) {
    struct v4l2_buffer buf;

    CLEAR (buf);

    buf.type        = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory      = V4L2_MEMORY_MMAP;
    buf.index       = i;

    if (-1 == xioctl_(fd, (int)VIDIOC_QBUF, &buf)){
      BIASERR("error: ioctl VIDIOC_QBUF:"<<strerror(errno));
      return -1;
    }
  }

  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (-1 == xioctl_(fd, VIDIOC_STREAMON, &type)){
    //BEXCEPTION("Failed to activate streaming from device :"<<strerror(errno))
    BIASERR("Failed to activate streaming from device :"<<strerror(errno));
    return -2;
  }
  return 0;
}

int VideoSource_V4L::
StopCapturing_()
{
  enum v4l2_buf_type type;
  type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (-1 == xioctl_(fd, VIDIOC_STREAMOFF, &type))
    BEXCEPTION("Failed to deactivate streaming from device :"<<strerror(errno))


  return 0;
}


int VideoSource_V4L::
UninitMMap_()
{
  //  cout << "UninitMMap_()\n";
  const unsigned num_bufs = Buffers_.size();
  for (unsigned i = 0; i < num_bufs; ++i){
    if (-1 == munmap (Buffers_[i].start, Buffers_[i].length))
        BEXCEPTION("Failed unmap memory :"<<strerror(errno))
  }
  return 0;
}


int VideoSource_V4L::
ReadFrame_()
{
  struct v4l2_buffer buf;

  CLEAR(buf);

  buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  buf.memory = V4L2_MEMORY_MMAP;

  if (-1 == xioctl_(fd, (int)VIDIOC_DQBUF, &buf)) {
    switch (errno) {
    case EAGAIN:
      cerr << "cannot DQBUF\n";
      return -1;
    case EIO:
      /* Could ignore EIO, see spec. */
      /* fall through */
    default:
      perror("ioctl: VIDIOC_DQBUF");
      return -1;
    }
  }
  assert(buf.index < Buffers_.size());

  pthread_mutex_lock(&ActiveFrameMutex_);
  //cout << "filling Frame_["<<(int)(!ActiveFrame_)<<"] ... ";
  assert(ActiveFrame_==0 || ActiveFrame_==1);
  assert((!ActiveFrame_)==0 || (!ActiveFrame_)==1);
  assert(Frame_.size()==2);
  unsigned num = Frame_[!ActiveFrame_].GetPixelCount()*
    Frame_[!ActiveFrame_].GetChannelCount()*Frame_[!ActiveFrame_].GetDepth();
  
  if (num !=  Buffers_[buf.index].length){
    Frame_[!ActiveFrame_].PrintHeader();
  
    if (isJPEG_) {
#ifdef BIAS_HAVE_LIBJPEG        
         Decompressor_.SetCompressedData((void *)&(Buffers_[buf.index]), 
                 (long) Buffers_[buf.index].length);
        /* 
         struct v4lconvert_data data;
         int res = v4lconvert_needs_conversion(&data,format2_,formatRGB_);
         if(res != 0){
             jpeg_.Release(false);
             jpeg_.Init(GetWidth(),GetHeight(),GetColorChannels());
             unsigned lengthJPEG;
             lengthJPEG =  jpeg_.GetPixelCount()*jpeg_.GetChannelCount()*jpeg_.GetDepth();
             unsigned char * idJPEG = jpeg_.GetImageData();
             res = v4lconvert_convert(&data,format2_,formatRGB_,
                     &(Buffers_[buf.index]),Buffers_[buf.index].length,idJPEG,lengthJPEG);
         }
         */
         if (Decompressor_.Decompress(jpegImg_,true) < 0) {
             BIASERR("Decompression failed!");
             return -1;
         }
         if (Frame_[!ActiveFrame_].SamePixelAndChannelCount(jpegImg_))
             Frame_[!ActiveFrame_].CopyIn_NoInit(jpegImg_.GetImageData());
         else 
             Frame_[!ActiveFrame_]=jpegImg_;
#endif
    }
    else{
      cerr << "buffer and image size mismatch: "<<num<<"\t"
             <<Buffers_[buf.index].length<<endl;
        BIASASSERT(num <=  Buffers_[buf.index].length);
    }
  }
  else{
    Frame_[!ActiveFrame_].
            CopyIn_NoInit((unsigned char*)Buffers_[buf.index].start);
  }
  
  ActiveFrame_ = !ActiveFrame_;
  newFrame_ = true;
  if (!ReadFirstFrame_) ReadFirstFrame_=true;
  //cout << "done\n";
  pthread_mutex_unlock(&ActiveFrameMutex_);

  if (-1 == xioctl_(fd, (int)VIDIOC_QBUF, &buf)){
    cerr << "ioctl: VIDIOC_QBUF\n";
    perror("ioctl: VIDIOC_QBUF");
    return -1;
  }

  return 0;
}


int VideoSource_V4L::
InitDevice_()
{
  struct v4l2_capability cap;
  struct v4l2_cropcap cropcap;
  struct v4l2_crop crop;
  struct v4l2_format fmt;
  unsigned int min;

  if (-1 == xioctl_(fd, (int)VIDIOC_QUERYCAP, &cap)) {
    if (EINVAL == errno) {
      perror("not a V4L2 device");
      return -1;
    } else {
      perror("InitDevice_() ioctl: VIDIOC_QUERYCAP");
      return -2;
    }
  }
  Identifier_ = (char*)cap.card;
  FixIdentifierString_();

  if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
    BEXCEPTION("No video capture device");

  if (!(cap.capabilities & V4L2_CAP_STREAMING)) 
    BEXCEPTION("This device does not support streaming i/o");

  /* Select video input, video standard and tune here. */
  CLEAR (cropcap);

  cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;

  if (0 == xioctl_(fd, (int)VIDIOC_CROPCAP, &cropcap)) {
    crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    crop.c = cropcap.defrect; /* reset to default */

    if (-1 == xioctl_(fd, VIDIOC_S_CROP, &crop)) {
      switch (errno) {
      case EINVAL:
        /* Cropping not supported. */
        break;
      default:
        /* Errors ignored. */
        break;
      }
    }
  } else {
    /* Errors ignored. */
  }


  CLEAR (fmt);

  fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  fmt.fmt.pix.width       = Width_;
  fmt.fmt.pix.height      = Height_;
  // cout<<"Opening with colormode:"<<ColorMode_<<" and:"<<Width_<<"x"<<Height_<<" pixel"<<endl;

  switch (ColorMode_) {
  case ImageBase::CM_Grey:
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_GREY;
    break;
  case ImageBase::CM_YUYV422:
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    break;
  case ImageBase::CM_RGB:
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
    break;
  case ImageBase::CM_BGR:
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_BGR24;
    break;
  case ImageBase::CM_BGRA:
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_BGR32;
    break;
  case ImageBase::CM_Bayer_GBRG:
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SGBRG8;
    break;
  case  ImageBase::CM_Bayer_BGGR:
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_SBGGR8;
    break;
  default:
    BEXCEPTION("Unsupported color model: "<<ColorMode_);
    break;
  }
  //why are we always using interlaced images??
  //todo add check for interlaced images and set here correctly
  fmt.fmt.pix.field       = interlaceMode_;

  // check if the requested format exists
  if (-1 == xioctl_(fd, (int)VIDIOC_TRY_FMT, &fmt)){
    BIASERR("Unable to set video device to requested format, "
            "falling back to 640x480 YUYV");
    CLEAR (fmt);
    fmt.type                = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width       = 640;
    fmt.fmt.pix.height      = 480;
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
    fmt.fmt.pix.field       = V4L2_FIELD_ANY;
  }

  if (-1 == xioctl_(fd, (int)VIDIOC_S_FMT, &fmt))
    BEXCEPTION("Failed to set video format: "<<strerror(errno));


  /* Note VIDIOC_S_FMT may change width and height. */

  /* Buggy driver paranoia. */
  min = fmt.fmt.pix.width * 2;
  if (fmt.fmt.pix.bytesperline < min)
    fmt.fmt.pix.bytesperline = min;
  min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
  if (fmt.fmt.pix.sizeimage < min)
    fmt.fmt.pix.sizeimage = min;

  Width_ = fmt.fmt.pix.width;
  Height_ = fmt.fmt.pix.height;
  BytesPerPixel_ = 1.; // only unsigned char images
  //cout<<fmt.fmt.pix.pixelformat<<" RGB:"<< V4L2_PIX_FMT_RGB24 <<endl;
  switch (fmt.fmt.pix.pixelformat){
  case V4L2_PIX_FMT_GREY:
    ColorMode_ = ImageBase::CM_Grey;
    ColorChannels_ = 1;
    break;
  case V4L2_PIX_FMT_YUYV:
    ColorMode_ = ImageBase::CM_YUYV422;
    ColorChannels_ = 2;
    break;
  case V4L2_PIX_FMT_RGB24:
    ColorMode_ = ImageBase::CM_RGB;
    ColorChannels_ = 3;
    break;
  case V4L2_PIX_FMT_BGR24:
    ColorMode_ = ImageBase::CM_BGR;
    ColorChannels_ = 3;
    break;
  case V4L2_PIX_FMT_BGR32:
    cout<<"Set color mode to BGRA"<<endl;
    ColorMode_ = ImageBase::CM_BGRA;
    ColorChannels_ = 4;
    break;
  case V4L2_PIX_FMT_SGBRG8:
    cout<<"Thats it Bayer GBRG"<<endl;
    ColorMode_ = ImageBase::CM_Bayer_GBRG;
    ColorChannels_ = 2;
    break;
  case V4L2_PIX_FMT_JPEG:
    cout<<"Thats it JPEG/JFIF"<<endl;
    ColorMode_ = ImageBase::CM_Grey;
    ColorChannels_ = 1;
    struct v4l2_jpegcompression compression;
    if (xioctl_(fd, (int)VIDIOC_G_JPEGCOMP, &compression)==0) {
        cout<<"Got jpeg compression params"<<endl;
        cout<<"compression.quality:"<<compression.quality<<endl;
        cout<<"compression.APPn:"<<compression.APPn<<endl;
        cout<<"compression.APP_len:"<<compression.APP_len<<endl;
        cout<<"compression.COM_len:"<<compression.COM_len<<endl;
        isJPEG_ = true;
    }
    else BIASERR("Could not get jpeg compression parameters.");
 
    break;
  default:
    BIASERR("unknown color model:"<<fmt.fmt.pix.pixelformat);
    return -5;
    break;
  }

  struct v4l2_queryctrl controls;
  //  for (controls.id=V4L2_CID_BASE; controls.id<V4L2_CID_LASTP1;
  //       controls.id++) {
  for (controls.id=V4L2_CID_BASE; controls.id<V4L2_CID_LASTP1-23;
        controls.id++) { // for the sony PTZ
    unsigned int tmpID = controls.id;
    if(controls.id == V4L2_CID_BASE+4) continue;
    if(controls.id == V4L2_CID_BASE+5) continue;
    //    if(controls.id == V4L2_CID_BASE+10) continue;
    //    ...
    //    if(controls.id == V4L2_CID_BASE+32) continue;
    if (xioctl_(fd, (int)VIDIOC_QUERYCTRL, &controls)==0) {
#ifdef BIAS_DEBUG
        cout <<"control valid: V4L2_CID_BASE+"<<controls.id-V4L2_CID_BASE
                <<" name: "<<controls.name<<endl;
#endif
        switch (controls.id){
        case V4L2_CID_BRIGHTNESS:
            memcpy(&ControlBrightness_,&controls, sizeof(controls));
            break;
        case V4L2_CID_CONTRAST:
            memcpy(&ControlContrast_,&controls, sizeof(controls));
            break;
        case V4L2_CID_GAIN:
            memcpy(&ControlGain_,&controls, sizeof(controls));
            break;
        case V4L2_CID_EXPOSURE:
            memcpy(&ControlShutter_,&controls, sizeof(controls));
            break;
        case V4L2_CID_AUTOGAIN:
            memcpy(&ControlAutoGain_,&controls, sizeof(controls));
            break;
        }
    }
    // else
    //   cout <<"control invalid: V4L2_CID_BASE+"<<controls.id-V4L2_CID_BASE<<endl;
    if(tmpID != controls.id){
        BIASERR("xioctl_ destroyed struct at ID: " << tmpID << " BaseID: " << V4L2_CID_BASE << " EndID: "  << V4L2_CID_LASTP1 << " ID-BaseID: " << tmpID-V4L2_CID_BASE << endl);
      return -6;
    }
  }

  /* Damned f.... Logitech QC does not support these fine controls

     for (controls.id=V4L2_CID_CAMERA_CLASS_BASE; controls.id<=V4L2_CID_PRIVACY;
     controls.id++) {
     if (xioctl_(fd, (int)VIDIOC_QUERYCTRL, &controls)==0) {
     cout <<"control valid: "<<controls.id<<" name: "<<controls.name<<endl;
     switch (controls.id){
     case V4L2_CID_ZOOM_ABSOLUTE:
     cout <<"Have Control Zoom absolute"<<endl; 
     memcpy(&ControlZoom_,&controls, sizeof(controls)); 
     break;
     case V4L2_CID_ZOOM_RELATIVE:
     cout <<"Have Control Zoom realtive"<<endl; 
     break;
     case V4L2_CID_PAN_RESET:
     cout <<"Have Control pan reset "<<endl; 
     memcpy(&ControlPanReset_,&controls, sizeof(controls)); 
     break;
     case V4L2_CID_TILT_RESET:
     cout <<"Have Control tilt reset "<<endl; 
     memcpy(&ControlTiltReset_,&controls, sizeof(controls)); 
     break;
     case V4L2_CID_PAN_ABSOLUTE:
     cout <<"Have Control pan  absolute"<<endl; 
     break;
     case V4L2_CID_PAN_RELATIVE:
     cout <<"Have Control pan  relative"<<endl; 
     break;
     }
     }
     else
     cout <<"control invalid: V4L2_CID_CAMERA_CLASS_BASE +"<<controls.id-V4L2_CID_CAMERA_CLASS_BASE <<endl;
     }
   */

  format2_ = fmt;
  formatRGB_ = fmt;
  formatRGB_.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
  return 0;
}


#undef CLEAR

int VideoSource_V4L::
SetGain(float g)
{
  if (! CompleteInitialized_) return -1.0f;
  if (ControlGain_.id ==0) return -2.0f;
  struct v4l2_control ctrl;
  ctrl.id = ControlGain_.id;

  float min = ControlGain_.minimum;
  float max = ControlGain_.maximum;
  float val = (g /100.0f) * (max-min) + min;
  ctrl.value = (int) rint(val);

  if (xioctl_(fd, (int)VIDIOC_S_CTRL, &ctrl)<0) {
    perror("SetGain");
    return -3.0f;
  }
  else cout <<"Set Brightness to: "<<ctrl.value<<endl;
  return 0;
}



float VideoSource_V4L::GetGain()
{
  if (! CompleteInitialized_) return -1.0f;
  if (ControlGain_.id ==0) return -2.0f;
  struct v4l2_control ctrl;
  ctrl.id = ControlGain_.id;
  if (xioctl_(fd, (int)VIDIOC_G_CTRL, &ctrl)<0) {
    perror("GetGain");
    return -3.0f;
  }

  cout <<"Gain val: "<<ctrl.value<<" min: "<<ControlGain_.minimum<<" max: "<<ControlGain_.maximum<<endl;
  float val = ctrl.value;
  float min = ControlGain_.minimum;
  float max = ControlGain_.maximum;

  val = ((val-min) / (max-min))*100.0f;
  return val;
}

void VideoSource_V4L::
SetAutoShutter(bool b)
{
}

bool VideoSource_V4L::
GetAutoShutter()
{
  return false;
}

void VideoSource_V4L::
SetAutoGain(bool b)
{
}


bool VideoSource_V4L::
GetAutoGain()
{
  return false;
}

int VideoSource_V4L::
SetBrightness(float g)
{
  if (! CompleteInitialized_) return -1.0f;
  if (ControlBrightness_.id ==0) return -2.0f;
  struct v4l2_control ctrl;
  ctrl.id = ControlBrightness_.id;

  float min = ControlBrightness_.minimum;
  float max = ControlBrightness_.maximum;
  float val = (g/100.0f) * (max-min) + min;
  ctrl.value = (int) rint(val);

  if (xioctl_(fd, (int)VIDIOC_S_CTRL, &ctrl)<0) {
    perror("SetBrightness");
    return -3.0f;
  }
 return 0;
}

float VideoSource_V4L::
GetBrightness()
{
  if (! CompleteInitialized_) return -1.0f;
  if (ControlBrightness_.id ==0) return -2.0f;
  struct v4l2_control ctrl;
  ctrl.id = ControlBrightness_.id;
  if (xioctl_(fd, (int)VIDIOC_G_CTRL, &ctrl)<0) {
    perror("GetBrightness");
    return -3.0f;
  }
 
  float val = ctrl.value;
  float min = ControlBrightness_.minimum;
  float max = ControlBrightness_.maximum;
  val = ((val-min) / (max-min))*100.0f;
  return val;
}

int VideoSource_V4L::
SetContrast(float g)
{
  if (! CompleteInitialized_) return -1.0f;
  if (ControlContrast_.id ==0) return -2.0f;
  struct v4l2_control ctrl;
  ctrl.id = ControlContrast_.id;

  float min = ControlContrast_.minimum;
  float max = ControlContrast_.maximum;
  float val = (g/100.0f) * (max-min) + min;
  ctrl.value = (int) rint(val);

  if (xioctl_(fd, (int)VIDIOC_S_CTRL, &ctrl)<0) {
    perror("SetContrast");
    return -3.0f;
  }
 return 0;
}


float VideoSource_V4L::
GetContrast()
{
  if (! CompleteInitialized_) return -1.0f;
  if (ControlContrast_.id ==0) return -2.0f;
  struct v4l2_control ctrl;
  ctrl.id = ControlContrast_.id;
  if (xioctl_(fd, (int)VIDIOC_G_CTRL, &ctrl)<0) {
    perror("GetContrast");
    return -3.0f;
  }
  float val = ctrl.value;
  float min = ControlContrast_.minimum;
  float max = ControlContrast_.maximum;

  val = ((val-min) / (max-min))*100.0f;
  return val;
}

int VideoSource_V4L::
SetShutter(float g)
{
  if (! CompleteInitialized_) return -1.0f;
  if (ControlShutter_.id ==0) return -2.0f;
  struct v4l2_control ctrl;
  ctrl.id = ControlShutter_.id;

  float min = ControlShutter_.minimum;
  float max = ControlShutter_.maximum;
  float val = (g/1.0f) * (max-min) + min;
  ctrl.value = (int) rint(val);

  if (xioctl_(fd, (int)VIDIOC_S_CTRL, &ctrl)<0) {
    perror("SetShutter");
    return -3.0f;
  }
  return 0;
}


float VideoSource_V4L::
GetShutter()
{
  if (! CompleteInitialized_) return -1.0f;
  if (ControlShutter_.id ==0) return -2.0f;
  struct v4l2_control ctrl;
  ctrl.id = ControlShutter_.id;
  if (xioctl_(fd, (int)VIDIOC_G_CTRL, &ctrl)<0) {
    perror("GetShutter");
    return -3.0f;
  }
  float val = ctrl.value;
  float min = ControlShutter_.minimum;
  float max = ControlShutter_.maximum;

  val = ((val-min) / (max-min))*1.0f;
  return val;

}

bool VideoSource_V4L::
HasControlBrightness()
{
  return (ControlBrightness_.id !=0);
}

bool VideoSource_V4L::
HasControlContrast()
{
  return (ControlContrast_.id !=0);
}

bool VideoSource_V4L::
HasControlShutter()
{
  return (ControlShutter_.id !=0);
}

bool VideoSource_V4L::
HasControlGain()
{
  return (ControlGain_.id !=0);
}

void VideoSource_V4L::
ResetPanTiltZoom()
{

}