VideoSource_DSHOW.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 "VideoSource_DSHOW.hh"
#include <Base/Image/ImageIO.hh>
#include <Base/Image/ImageConvert.hh>
#include <ks.h>
#include <ksmedia.h>

using namespace BIAS;
using namespace std;


#ifdef BIAS_HAVE_PTHREADS
#include <pthread.h>
pthread_mutex_t imageMutex_;
#endif

bool                              UpSideDown_;

VideoSource_DSHOW::VideoSource_DSHOW(bool bShowDialogs): VideoSource()
{
  #ifdef BIAS_HAVE_PTHREADS
  pthread_mutex_init (&imageMutex_,NULL);
  #endif

  dWidth_                 = 640;
    dHeight_                = 480;
  UpSideDown_             = true; //RGB default
  OverrideColorMode_      = false; // ignore SetColorModel(), accept cameras own CM
    dDepth_                 = 1;
    dDsBufferSize_          = 0;
  pDsBuffer_              = NULL;
    pMediaControl_          = NULL;
    pMediaEvent_            = NULL;
    pGraphBuilder_          = NULL;
  pCaptureGraphBuilder2_  = NULL;
    pGrabber_               = NULL;
    pNullRenderer_          = NULL;
  bShowDialogs_           = bShowDialogs;
  pImageBuffer_           = NULL;
  sCaptureDeviceName_     = "";
  BIASColorModel_         = BIAS::ImageBase::CM_BGR;
  MediaSubTypeColorModel_ = MEDIASUBTYPE_None;
  FGCB                    = NULL; //callback pointer
  GetCapabilitiesOnly_    = false;
  SetSizeAndColorModel(dWidth_,dHeight_,dDepth_,BIASColorModel_);
}

VideoSource_DSHOW::VideoSource_DSHOW(int width, int height,int depth,bool bShowDialogs): VideoSource()
{
  dWidth_                 = width;
    dHeight_                = height;
    dDepth_                 = depth;
  UpSideDown_             = true;//RGB default
    dDsBufferSize_          = 0;
  pDsBuffer_              = NULL;
    pMediaControl_          = NULL;
    pMediaEvent_            = NULL;
    pGraphBuilder_          = NULL;
  pCaptureGraphBuilder2_  = NULL;
    pGrabber_               = NULL;
    pNullRenderer_          = NULL;
  bShowDialogs_           = bShowDialogs;
  pImageBuffer_           = NULL;
  sCaptureDeviceName_     = "";
  BIASColorModel_         = BIAS::ImageBase::CM_BGR;
  MediaSubTypeColorModel_ = MEDIASUBTYPE_None;
  FGCB                    = NULL; //callback pointer
  GetCapabilitiesOnly_    = false;
  SetSizeAndColorModel(dWidth_,dHeight_,dDepth_,BIASColorModel_);
}


VideoSource_DSHOW::~VideoSource_DSHOW()
{
  if(pImageBuffer_!=NULL)
    delete [] pImageBuffer_;
  if(pDsBuffer_!=NULL)
    delete [] pDsBuffer_;
  if(FGCB != NULL)
    delete FGCB;
}

int VideoSource_DSHOW::InitImage(BIAS::ImageBase &Image){
  if (!Image.IsEmpty()){
    BIASWARNONCE("VideoSource::InitImage() should be called with uninitialized image");
    Image.Release();
  }
  Image.Init(GetWidth(), GetHeight(),GetColorChannels());
  Image.SetColorModel(BIASColorModel_);
  
  return 0;
}

int VideoSource_DSHOW::OpenDevice()
{
  return OpenDevice("");
}

int VideoSource_DSHOW::OpenDevice(const char * device)
{
  sCaptureDeviceName_ = device;
  int ret=0;  
  try{
    ret = InitVideoCapture_();
  }catch(BIAS::BaseException e){
    BIASERR("Error initialising Video capture:"<<e.what());
  }catch(...){
    BIASERR("Error initialising Video capture!");
  }
  return ret;  
}

int VideoSource_DSHOW::OpenDevice(unsigned int width, unsigned int height, unsigned int channels,
    const char * device,BIAS::ImageBase::EColorModel model/* = BIAS::ImageBase::CM_RGB*/)
{
  sCaptureDeviceName_ = device;
  SetSize(width,height,channels);
  SetColorModel(model);
  int ret=0;  
  try{
    ret = InitVideoCapture_();
  }catch(BIAS::BaseException e){
    BIASERR("Error initialising Video capture:"<<e.what());
  }catch(...){
    BIASERR("Error initialising Video capture!");
  }
  return ret;  
}
 
int VideoSource_DSHOW::CloseDevice()
{
  if (pGrabber_) pGrabber_.Release(); 
  if (pNullRenderer_) pNullRenderer_.Release();
  if (pSrcFilter_) pSrcFilter_.Release();
  if (pMediaControl_) {
    pMediaControl_->Stop();
    pMediaControl_.Release();
  }
  if (pCameraControl_) pCameraControl_.Release();
  if (pVideoProcControl_) pVideoProcControl_.Release();
  if (pMediaEvent_) pMediaEvent_.Release();
  if (pGraphBuilder_) pGraphBuilder_.Release();
  if (pCaptureGraphBuilder2_) pCaptureGraphBuilder2_.Release();
  pOutPin_.Release();
  pSampleOutPin_.Release();
  return 0;
}


int VideoSource_DSHOW::GrabSingle(BIAS::Camera <unsigned char> &image)
{
  long evCode=0;
  LONG_PTR p1=NULL,p2=NULL;
  HRESULT hr=0;
  hr = pMediaEvent_->GetEvent(&evCode, &p1,&p2,1);
  if (hr == S_OK) { // ok means: Event found.
    switch  (evCode) {
     case EC_DEVICE_LOST: BEXCEPTION("Camera diconnected."); break; 
    }
  }
  pMediaEvent_->FreeEventParams(evCode, p1,p2);
  //lock image
  #ifdef BIAS_HAVE_PTHREADS
  pthread_mutex_lock(&imageMutex_); 
  #endif 
  while(!FGCB->HasNewImage()){
    //if no new image lock image and sleep
    #ifdef BIAS_HAVE_PTHREADS
    pthread_mutex_unlock(&imageMutex_); 
    #endif
    Sleep(1);
    #ifdef BIAS_HAVE_PTHREADS
    //after wakeup lock again %
    pthread_mutex_lock(&imageMutex_); 
    #endif
  }
  image.CopyIn_NoInit(FGCB->GetImage().GetImageData());
   //static int count = 0;
   //ostringstream oss;
   //oss << "_flip_"<<setw(3)<<setfill('0')<<count++<<".mip";
  //ImageIO::Save("before"+oss.str(), image);
  //ImageIO::Save("after"+oss.str(), image);
  image.SetUID(BIAS::UUID::GenerateUUID());
  #ifdef BIAS_HAVE_PTHREADS
  //after wakeup lock again %
  pthread_mutex_unlock(&imageMutex_); 
  #endif
  
  return 0;

}


void VideoSource_DSHOW::DeleteMediaType_(AM_MEDIA_TYPE *pmt)
{
    // allow NULL pointers for coding simplicity

    if (pmt == NULL) {
        return;
    }

    if (pmt->cbFormat != 0) {
        CoTaskMemFree((PVOID)pmt->pbFormat);

        // Strictly unnecessary but tidier
        pmt->cbFormat = 0;
        pmt->pbFormat = NULL;
    }
    if (pmt->pUnk != NULL) {
        pmt->pUnk->Release();
        pmt->pUnk = NULL;
    }

    CoTaskMemFree((PVOID)pmt);
}


int VideoSource_DSHOW::PreGrab()
{
  return 0;
}

int VideoSource_DSHOW::PostGrab()
{
  while(pthread_mutex_trylock(&imageMutex_)==EBUSY)
  Sleep(1);
  pthread_mutex_destroy(&imageMutex_);
  return 0;
}

bool VideoSource_DSHOW::DisplayPinProperties_( CComPtr<IPin> pSrcPin )
{  
    CComPtr<ISpecifyPropertyPages> pPages;

    HRESULT hr = pSrcPin->QueryInterface(IID_ISpecifyPropertyPages, (void**)&pPages);
    if (SUCCEEDED(hr))
    {
        PIN_INFO PinInfo;
        pSrcPin->QueryPinInfo(&PinInfo);

        CAUUID caGUID;
        pPages->GetPages(&caGUID);

        OleCreatePropertyFrame(
            NULL,
            0,
            0,
            L"Property Sheet",
            1,
            (IUnknown **)&(pSrcPin),
            caGUID.cElems,
            caGUID.pElems,
            0,
            0,
            NULL);

        CoTaskMemFree(caGUID.pElems);
        PinInfo.pFilter->Release();
    }
    return false;
}

bool VideoSource_DSHOW::DisplayFilterProperties( CComPtr<IBaseFilter> pFilter)
{
    CComPtr<ISpecifyPropertyPages> pProp;
    HRESULT hr = pFilter->QueryInterface(IID_ISpecifyPropertyPages, (void **)&pProp);
    if (SUCCEEDED(hr)) 
    {
        // Get the filter's name and IUnknown pointer.
        FILTER_INFO FilterInfo;
        pFilter->QueryFilterInfo(&FilterInfo); 

        // Show the page. 
        CAUUID caGUID;
        pProp->GetPages(&caGUID);
        OleCreatePropertyFrame(
            NULL,                   // Parent window
            0, 0,                   // (Reserved)
            FilterInfo.achName,     // Caption for the dialog box
            1,                      // Number of objects (just the filter)
            (IUnknown **)&pFilter,  // Array of object pointers. 
            caGUID.cElems,          // Number of property pages
            caGUID.pElems,          // Array of property page CLSIDs
            0,                      // Locale identifier
            0, NULL);               // Reserved

        // Clean up.
        if(FilterInfo.pGraph != NULL) 
            FilterInfo.pGraph->Release(); 
        CoTaskMemFree(caGUID.pElems);
        return( true );
    }
    else
        return( false );
}

void VideoSource_DSHOW::DisplaySourcePinProperties()
{
  if(pOutPin_)
  {
      DisplayPinProperties_(pOutPin_);
  }
}


bool VideoSource_DSHOW::DisplayCameraProperties()
{
  IBaseFilter * pFilter = pSrcFilter_;
    CComPtr<ISpecifyPropertyPages> pProp;
    HRESULT hr = pFilter->QueryInterface(IID_ISpecifyPropertyPages, (void **)&pProp);
    if (SUCCEEDED(hr)) 
    {
        // Get the filter's name and IUnknown pointer.
        FILTER_INFO FilterInfo;
        pFilter->QueryFilterInfo(&FilterInfo); 

        // Show the page. 
        CAUUID caGUID;
        pProp->GetPages(&caGUID);
        OleCreatePropertyFrame(
            NULL,                   // Parent window
            0, 0,                   // (Reserved)
            FilterInfo.achName,     // Caption for the dialog box
            1,                      // Number of objects (just the filter)
            (IUnknown **)&pFilter,  // Array of object pointers. 
            caGUID.cElems,          // Number of property pages
            caGUID.pElems,          // Array of property page CLSIDs
            0,                      // Locale identifier
            0, NULL);               // Reserved

        // Clean up.
        if(FilterInfo.pGraph != NULL) 
            FilterInfo.pGraph->Release(); 
        CoTaskMemFree(caGUID.pElems);
        return( true );
    }
    else
        return( false );
}

///////////////////////////////////////////////////////////////
void VideoSource_DSHOW::DetermineColorModel_()
{
  if(BIASColorModel_ != BIAS::ImageBase::CM_invalid)
  {
    if(BIASColorModel_ == BIAS::ImageBase::CM_RGB)
      MediaSubTypeColorModel_ = MEDIASUBTYPE_RGB24;
    if(BIASColorModel_ == BIAS::ImageBase::CM_RGBA)
      MediaSubTypeColorModel_ = MEDIASUBTYPE_RGB32;
    if(BIASColorModel_ == BIAS::ImageBase::CM_Grey)
      MediaSubTypeColorModel_ = MEDIASUBTYPE_RGB1;
    if(BIASColorModel_ == BIAS::ImageBase::CM_UYVY422)
      MediaSubTypeColorModel_ = MEDIASUBTYPE_UYVY;
    if(BIASColorModel_ == BIAS::ImageBase::CM_YUV420P)
      MediaSubTypeColorModel_ = MEDIASUBTYPE_Y41P;
  }
  else
    MediaSubTypeColorModel_ = MEDIASUBTYPE_None;
}

void VideoSource_DSHOW::DetermineBIASColorModel_()
{
  if(MediaSubTypeColorModel_ != MEDIASUBTYPE_None)
  {
    if(MediaSubTypeColorModel_ == MEDIASUBTYPE_RGB24 || MediaSubTypeColorModel_== MEDIASUBTYPE_RGB32)
      BIASColorModel_ = BIAS::ImageBase::CM_BGR;
    else if(MediaSubTypeColorModel_ == MEDIASUBTYPE_RGB1)
      BIASColorModel_ = BIAS::ImageBase::CM_Grey;   
    else if(MediaSubTypeColorModel_ == MEDIASUBTYPE_UYVY)
      BIASColorModel_ = BIAS::ImageBase::CM_UYVY422;
    else if(MediaSubTypeColorModel_ == MEDIASUBTYPE_Y41P)
      BIASColorModel_ = BIAS::ImageBase::CM_YUV420P;
    else if(MediaSubTypeColorModel_ == MEDIASUBTYPE_IYUV)
      BIASColorModel_ = BIAS::ImageBase::CM_YUV411;
    else if(MediaSubTypeColorModel_ ==MEDIASUBTYPE_Y411)
      BIASColorModel_ = BIAS::ImageBase::CM_YUV411;
    else if(MediaSubTypeColorModel_ ==MEDIASUBTYPE_Y211)
      BIASColorModel_ = BIAS::ImageBase::CM_YUV420P;
    else if(MediaSubTypeColorModel_ ==MEDIASUBTYPE_YUY2)
      BIASColorModel_ = BIAS::ImageBase::CM_YUYV422;
    else
      BIASColorModel_ = BIAS::ImageBase::CM_invalid;
  }
  else
    BIASColorModel_ = BIAS::ImageBase::CM_RGB;
}

HRESULT VideoSource_DSHOW::InitVideoCapture_()
{
  HRESULT hr=S_OK;
  // initialize the COM library.    
  CoInitialize(0);

  // Get DirectShow interfaces
  hr = GetInterfaces_();
  if (S_OK!=hr)
    BEXCEPTION( "Failed to get DirectShow interfaces! ");

  // Use the system device enumerator and class enumerator to find
  // a video capture/preview device, such as a desktop USB video camera.
  hr = FindCaptureDevice_(&pSrcFilter_);
  if (S_OK!=hr)
  {
    // Don't display a message because FindCaptureDevice will handle it
    return hr;
  }
    
  // Add Capture filter to our graph.
  hr = pGraphBuilder_->AddFilter(pSrcFilter_, L"Video Capture");
  if (S_OK!=hr)
  {
    pSrcFilter_.Release();
    BEXCEPTION("Couldn't add the capture filter to the graph! \n"<<
           "If you have a working video capture device, please make sure\n"<<
           "that it is connected and is not being used by another application.");

  }
  
  pOutPin_=NULL;
  // get the output pin of the Capture device filter
  if(pSrcFilter_->FindPin(L"Capture",&pOutPin_)!=S_OK)
    if (pSrcFilter_->FindPin(L"0",&pOutPin_)!=S_OK)
      if (pSrcFilter_->FindPin(L"Out",&pOutPin_)!=S_OK)
    if (pSrcFilter_->FindPin(L"1",&pOutPin_)!=S_OK)
      BEXCEPTION("Could not find Source Outpin");

  if( pOutPin_ ){
    //cout<< "Found VideoSource Outpin" << endl;
    if(bShowDialogs_)
      DisplayPinProperties_( pOutPin_ );
  } 
  else  BEXCEPTION("Did not fine outpin");
 
  //DisplayCameraProperties();
    
  //here determine size and set
  IAMStreamConfig * pConfig=0;
  hr = pCaptureGraphBuilder2_->FindInterface(&PIN_CATEGORY_CAPTURE,
                         &MEDIATYPE_Video,
                         pSrcFilter_, IID_IAMStreamConfig, 
                         (void **)&pConfig);
  if(hr != S_OK) {
    hr = pCaptureGraphBuilder2_->FindInterface(&PIN_CATEGORY_CAPTURE,
                           &MEDIATYPE_Interleaved, 
                           pSrcFilter_,
                           IID_IAMStreamConfig, 
                           (void **)&pConfig);
    if (hr !=S_OK)
      BEXCEPTION("FindInterface unsucessfull");
  }
  //----
  // define two media types to detect correct one
  AM_MEDIA_TYPE *pSourceVideoFormat = NULL, * pNewVideoFormat = NULL;
  int ccount,csize;
  bool validCamResFound = false;
  pConfig->GetNumberOfCapabilities(&ccount,&csize);
  BYTE* construct = new BYTE[csize];
  AM_MEDIA_TYPE* tmpMediaType; 
  for(int c=0; c<ccount; c++)
  {
    
    if(pConfig->GetStreamCaps(c,&tmpMediaType,construct)!=S_OK)
      BEXCEPTION("GetStreamCaps failed");
      
    if(tmpMediaType->majortype==MEDIATYPE_Video)
    {
      if((tmpMediaType->formattype==FORMAT_VideoInfo) )
/*        && 
        (tmpMediaType->cbFormat > sizeof(VIDEOINFOHEADER) &&
        (tmpMediaType->pbFormat != NULL)))
        */
      {   
        VIDEOINFOHEADER *vih = (VIDEOINFOHEADER *)tmpMediaType->pbFormat;

        if (GetCapabilitiesOnly_){
          ImageBase::EColorModel cm=ImageBase::CM_invalid;
          MediaSubTypeColorModel_ = tmpMediaType->subtype;
          DetermineBIASColorModel_();
          if (BIASColorModel_ != ImageBase::CM_invalid)
            Capabilities_.AddMode(vih->bmiHeader.biWidth,abs(vih->bmiHeader.biHeight),0,BIASColorModel_);
          continue;
        }
        // extract sign and store separately
        ImageHeightSign_ = vih->bmiHeader.biHeight / abs(vih->bmiHeader.biHeight);
        if((vih->bmiHeader.biWidth==GetWidth())&&
          (abs(vih->bmiHeader.biHeight)==GetHeight()))
        {
          cout <<"Matched width and height: "<<vih->bmiHeader.biWidth<<"x"<<vih->bmiHeader.biHeight<<endl;
          pSourceVideoFormat = tmpMediaType;  //store 

          if (OverrideColorMode_) {
          if(ColorMode_ == ImageBase::CM_RGB || ColorMode_ == ImageBase::CM_BGR) { // RGB requested
            if((tmpMediaType->subtype==MEDIASUBTYPE_RGB24)||(tmpMediaType->subtype==MEDIASUBTYPE_RGB32))   {
              cout <<"Found requested RGB type"<<endl;
              validCamResFound = true;
              break;
            }
          }
           if(ColorMode_ == ImageBase::CM_YUYV422) { //YUYV422 requested
             if(tmpMediaType->subtype==MEDIASUBTYPE_YUY2)   {
               cout <<"Found requested YUYV422"<<endl;
               UpSideDown_ = false; // YUV is always up-right, dont flip.
               validCamResFound = true;
              break;
             }
           }
          } // if(overrideColorMode)
          else {
            if((tmpMediaType->subtype==MEDIASUBTYPE_RGB24)
              ||(tmpMediaType->subtype==MEDIASUBTYPE_RGB32))   {
              cout <<"Accepting preferred RGB color model from camera"<<endl;
              ColorMode_ = ImageBase::CM_RGB;
              UpSideDown_ = true; // RGB is always upsidedown
              validCamResFound = true;
              break;
            }
            if(tmpMediaType->subtype==MEDIASUBTYPE_YUY2)   {
              cout <<"Accepting YUV422 color model from camera"<<endl;
              ColorMode_ = ImageBase::CM_YUYV422;
               UpSideDown_ = false; // YUV is always up-right, dont flip.
              validCamResFound = true;
              break;
            }

          }// else if(OverrideCOlorMode)
        } // if (width && height)
        else 
            cout <<"Unmatched width and height: "<<vih->bmiHeader.biWidth<<"x"<<vih->bmiHeader.biHeight<<endl;
      }// for ()
    }
   DeleteMediaType_(tmpMediaType);
  }
  delete construct;

  // if the task was to query all modes, we are finished.
  if (GetCapabilitiesOnly_) return 0; 

  if(!pSourceVideoFormat)
  {
    BEXCEPTION("no usable camera resolution format found");
  }
  if(!validCamResFound)
  {
    cout << "no required video resolution found\n"<<endl;
  }

  //if(hr = pConfig->GetFormat(&pSourceVideoFormat)) == S_OK) 
  {
    // Examine the media type for any information you need. 
    if(pSourceVideoFormat->majortype == MEDIATYPE_Video)
    {
      MediaSubTypeColorModel_ = pSourceVideoFormat->subtype;
      if(MediaSubTypeColorModel_ == MEDIASUBTYPE_RGB32)
      {
        MediaSubTypeColorModel_ = MEDIASUBTYPE_RGB24;
        pSourceVideoFormat->subtype = MediaSubTypeColorModel_;
      }         
    }
    else
    {  
      //get mediatype from videosource
      BEXCEPTION("The major format type is NOT video");
    }
    DetermineBIASColorModel_(); //determine corresponding BIAS ColorModel      

    BIASASSERT(pSourceVideoFormat->formattype==FORMAT_VideoInfo); 
    //pSourceVideoFormat->formattype = FORMAT_VideoInfo; //??? sonst gehts gut? 
    VIDEOINFOHEADER *vih = (VIDEOINFOHEADER *)pSourceVideoFormat->pbFormat;
    vih->bmiHeader.biWidth = GetWidth();
    vih->bmiHeader.biHeight = GetHeight() * ImageHeightSign_;
    cout << "requesting "<<vih->bmiHeader.biWidth<<"x"<<vih->bmiHeader.biHeight<<endl;
    vih->bmiHeader.biBitCount = (WORD)GetColorChannels()*8; 
    vih->bmiHeader.biSizeImage= DIBSIZE(vih->bmiHeader); //
    hr = pConfig->SetFormat( pSourceVideoFormat );
    if(hr!=S_OK){
      BEXCEPTION("Error Videoformat is inconsistent");
    } else {
      cout << "got requested format\n";
    }
  }
  DeleteMediaType_(pSourceVideoFormat);

//  pConfig->Release(); //really release here?

  IEnumMediaTypes * pMedia=0;
  vector<AM_MEDIA_TYPE *> vpfnt;

  //outpin is outpin from pSrcFilter_
  hr = pOutPin_->EnumMediaTypes( &pMedia );
  if( (hr = pOutPin_->EnumMediaTypes( &pMedia )) == S_OK)
  {
    cout<<"pOutPin_->EnumMediaTypes success"<< endl;
    bool FoundOne = false;
    while(pMedia->Next(1, &pSourceVideoFormat, 0) == S_OK && !FoundOne)
    {
      if( pSourceVideoFormat->formattype == FORMAT_VideoInfo )
      {
        VIDEOINFOHEADER *vih = (VIDEOINFOHEADER *)pSourceVideoFormat->pbFormat;
        int dCurWidth = vih->bmiHeader.biWidth;
        int dCurHeight = abs(vih->bmiHeader.biHeight);

        if( dCurWidth == GetWidth() && dCurHeight == GetHeight() )// && sfcc == "IYUV")
        {
          // this is the correct new Video Format
          pNewVideoFormat = pSourceVideoFormat;
          cout<<"copying pin format to newvideoformat"<<endl;
          FoundOne= true;
        }
        else
        {
          BEXCEPTION("No useful format on pin found");
        }
      }      
      else if( pSourceVideoFormat->formattype == FORMAT_None )
        cout<<"Format type: FORMAT_None"<<endl;
      else if( pSourceVideoFormat->formattype == FORMAT_DvInfo )
        cout<<"Format type: FORMAT_DvInfo"<<endl;
      else if( pSourceVideoFormat->formattype == FORMAT_MPEGVideo )
        cout<<"Format type: FORMAT_MPEGVideo"<<endl;
      else if( pSourceVideoFormat->formattype == FORMAT_MPEG2Video )
        cout<<"Format type: FORMAT_MPEG2Video"<<endl;
      else if( pSourceVideoFormat->formattype == FORMAT_VideoInfo2 )
        cout<<"Format type: FORMAT_VideoInfo2"<<endl;
    }
    pMedia->Release();
  }
  else
  {
    BEXCEPTION("pOutPin_->EnumMediaTypes failed");
  }

  //finally set the new videoFormat
  if( pNewVideoFormat != 0 )
  {
    hr = pConfig->SetFormat( pNewVideoFormat );
    if (hr !=S_OK)
    {
        BEXCEPTION("errro setting format");
    }
    int dWidth = ((VIDEOINFOHEADER *)pNewVideoFormat->pbFormat)->bmiHeader.biWidth;
    int dHeight = ((VIDEOINFOHEADER *)pNewVideoFormat->pbFormat)->bmiHeader.biHeight;
    double dDepth = ((VIDEOINFOHEADER *)pNewVideoFormat->pbFormat)->bmiHeader.biBitCount;
    if(dDepth == 12)
       dDepth = 2;
    else
      dDepth /= 8;
      SetSizeAndColorModel(dWidth,abs(dHeight),(int)dDepth,BIASColorModel_);    
      DeleteMediaType_( pNewVideoFormat );
    }
    
  // add sample grabber 
  CComQIPtr< IBaseFilter, &IID_IBaseFilter > pGrabberBase( pGrabber_ );
  if ( (hr =  pGraphBuilder_->AddFilter(pGrabberBase,L"SampleGrabber")) != S_OK)
    {
      BEXCEPTION("Couldn't add the grab filter.");
    }
  //set mediatype to sample grabber
  hr = pConfig->GetFormat( &pNewVideoFormat );
  if( (hr = pGrabber_->SetMediaType(pNewVideoFormat)) != S_OK )
    {
      BEXCEPTION("Setting Media Type failed!");
    }   
  DeleteMediaType_( pNewVideoFormat ); 
   

  // get the SampleGrabber Input Pin
  if (pGrabberBase->FindPin(L"In",&pInPin_) != S_OK)
    BEXCEPTION("Could not find SampleGrabber Input Pin");

  // connect SmartTee and SampleGrabber
  if ((hr = pGraphBuilder_->Connect(pOutPin_,pInPin_))!=S_OK)
    BEXCEPTION("Could not not connect Camera Source and SampleGrabber Pins")
  else
    cout<< "Succesfully connected Camera Source and SampleGrabber Pins" <<endl;

  // add the NullRenderer filter
  hr = pGraphBuilder_->AddFilter(pNullRenderer_,L"NullRenderer");
  if (FAILED(hr))
     BEXCEPTION("Couldn't add the grab filter.");
    

  // get the SampleGrabber Output Pin
  if (pGrabberBase->FindPin(L"Out",&pSampleOutPin_)!=S_OK)
    BEXCEPTION("Pin out of SampleGrabber not found");

  // get the NullRenderer Input Pin
  if (pNullRenderer_->FindPin(L"In",&pInPin_)!=S_OK)
    BEXCEPTION("Could not find NullRenderer Input Pin");
    
  // connect SampleGrabber and NullRenderer
  if (pGraphBuilder_->Connect(pSampleOutPin_,pInPin_)!= S_OK)
    BEXCEPTION("Could not not connect SampleGrabber and NullRenderer Pins")
  else
    cout<<"Succesfully connected SampleGrabber and NullRenderer Pins" << endl;
  
  pGrabber_->SetBufferSamples(FALSE);
  pGrabber_->SetOneShot(FALSE);
  if(FGCB == NULL)
    FGCB = new MySampleGrabberCB(GetWidth(),GetHeight(),GetColorChannels());
  FGCB->SetNewImageCheck(WaitForNew_);
  pGrabber_->SetCallback(FGCB,0);
  
  EnumFilters_(pGraphBuilder_);
  InitCamControls_();
  // Start video data source
  hr = pMediaControl_->Run();    
  if (FAILED(hr)) BEXCEPTION("Couldn't run the graph!  ");
 
  SaveGraphFile_(pGraphBuilder_,L"C:\\MyGraf.grf");
  return S_OK;
}

int VideoSource_DSHOW::
GetCapabilities(VideoSourceCapabilities &caps)
{
  return GetCapabilities("",caps);
}
 

int VideoSource_DSHOW::
GetCapabilities(const char *device, VideoSourceCapabilities &caps)
{
  int ret =0;
  sCaptureDeviceName_ = device;
  GetCapabilitiesOnly_ = true;
  try{
  ret = InitVideoCapture_();
  }catch(BIAS::BaseException e){
    BIASERR("Error initialising Video capture:"<<e.what());
  }
  catch(...){
    BIASERR("Error initialising Video capture!");
  }
  if(ret <0) return ret;

  GetCapabilitiesOnly_ = false;
  CloseDevice();
  caps = Capabilities_;
  return 0;
}


inline string ConvertBSTRToString(BSTR pSrc)
{
  DWORD cb,cwch = ::SysStringLen(pSrc);
  /*
  /////in datei kloppen zum testen
  ofstream os;
  os.open("E:\\USBIdentifier.txt");
  for ( unsigned i=0; i<cwch; i++ ) {
    os << pSrc[i] << endl;
  }
  os.flush();
  os.close();
  */
  char *szOut = NULL;
  string result;
  cwch = cwch+1;
  cb = ::WideCharToMultiByte(CP_ACP, 0, pSrc, cwch, NULL, 0, 0, 0);
  if(cb) {
      szOut = new char[cb];
      if(szOut) {
        szOut[cb - 1] = '\0';
        if(!::WideCharToMultiByte(CP_ACP, 0,pSrc, cwch, szOut, cb, 0, 0)) 
          delete []szOut; 
        else  {
          result = szOut;
          delete[] szOut;
        }
      }
  }
  return result;
}

int VideoSource_DSHOW::
GetAllDevices(vector<string> &devices) 
{
  devices.clear();
  HRESULT hr;
  ICreateDevEnum *pSysDevEnum = NULL;
  hr = CoCreateInstance(CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC_SERVER,
    IID_ICreateDevEnum, (void **)&pSysDevEnum);
  if (FAILED(hr))   return -1;
  
  IEnumMoniker *pEnumCat = NULL;
  hr = pSysDevEnum->CreateClassEnumerator((GUID)CLSID_VideoInputDeviceCategory, 
    &pEnumCat, 0);
  if (hr == S_OK)  {
    // Enumerate the monikers.
    IMoniker *pMoniker = NULL;
    ULONG cFetched;
    while(pEnumCat->Next(1, &pMoniker, &cFetched) == S_OK)  {
      IPropertyBag *pPropBag;
      hr = pMoniker->BindToStorage(0, 0, IID_IPropertyBag,
        (void **)&pPropBag);
      if (SUCCEEDED(hr))      {
        VARIANT varName;
        VariantInit(&varName);        
        hr = pPropBag->Read(L"Description", &varName, 0);
        if(FAILED(hr))
        {
          hr = pPropBag->Read(L"FriendlyName", &varName, 0);
        }  
        if (SUCCEEDED(hr))  {
          devices.push_back(ConvertBSTRToString(varName.bstrVal));
        }
      }
    }//while   
    pMoniker->Release();
  }//if
  pEnumCat->Release();    
  pSysDevEnum->Release();
  return 0;
}


HRESULT VideoSource_DSHOW::GetInterfaces_(void)
{
    HRESULT hr=S_OK;
    // Create the filter graph
    try{  
      hr = CoCreateInstance (CLSID_FilterGraph, NULL, CLSCTX_INPROC_SERVER,
                           IID_IGraphBuilder, (void **) &pGraphBuilder_);
    }catch(...){
      BIASERR("Could not create instance of Filtergraph for VideosourceDShow!");
      return -1;
    }
    if (hr!=S_OK) return hr;

    // Create the capture graph builder
    try{
      hr = CoCreateInstance(CLSID_CaptureGraphBuilder2 , NULL, CLSCTX_INPROC,
                            IID_ICaptureGraphBuilder2, (void **) &pCaptureGraphBuilder2_);
    }catch(...){
       BIASERR("Could not create instance of CaptureGraphBuilder for VideosourceDShow!");
       return -1;
    }
    if (hr!=S_OK)
        return hr;
    // Attach the filter graph to the capture graph
    hr = pCaptureGraphBuilder2_->SetFiltergraph(pGraphBuilder_);
    if (hr!=S_OK)
    {
        BIASERR("Failed to set capture filter graph!");
        return hr;
    }
   
    // Obtain interfaces for media control and Video Window
    hr = pGraphBuilder_->QueryInterface(IID_IMediaControl,(LPVOID *) &pMediaControl_);
    if (hr!=S_OK)
        return hr;

    hr = pGraphBuilder_->QueryInterface(IID_IMediaEvent, (LPVOID *) &pMediaEvent_);
    if (hr!=S_OK)
        return hr;

      // Create NullRenderer Instance
      hr = pNullRenderer_.CoCreateInstance(CLSID_NullRenderer);
      if (hr!=S_OK)
          return hr;

      // Create SampleGrabber Instance 
      // ISampleGrabber can Queried on this Filter to Grab Samples from the running Graph
      hr = pGrabber_.CoCreateInstance(CLSID_SampleGrabber);
      if(hr!=S_OK)
      {
      BIASERR("Could not create the SampleGrabber filter.");
          //MessageBox( GetDesktopWindow(), "Could not create the SampleGrabber filter.", "ImageSourceDShow", MB_OK | MB_ICONERROR ); 
      //    exit( -1 );
      BIASABORT;
    }

    return hr;
}

HRESULT VideoSource_DSHOW::FindCaptureDevice_(IBaseFilter ** ppSrcFilter)
{
  HRESULT hr;
  IBaseFilter * pSrc = NULL;
  ULONG cFetched=0;
    
  if (!ppSrcFilter)
    return E_POINTER;
   
  // Create the system device enumerator
  CComPtr <ICreateDevEnum> pDevEnum =NULL;

  hr = CoCreateInstance (CLSID_SystemDeviceEnum, NULL, CLSCTX_INPROC,
             IID_ICreateDevEnum, (void **) &pDevEnum);
  if(S_OK!=hr)
  {
    BIASERR("Couldn't create system enumerator!");
    return hr;
  }
  
  // Create an enumerator for the video capture devices
  IEnumMoniker* pClassEnum = 0;

  hr = pDevEnum->CreateClassEnumerator (CLSID_VideoInputDeviceCategory, &pClassEnum, 0);
  if(S_OK!=hr)
  {
    BIASERR("Couldn't create class enumerator! No DirectShow hardware?");
    return hr;
  }

  //////////////////// new
  pClassEnum->Reset();
  IMoniker *pM =0;
  while(hr = pClassEnum->Next(1, &pM, &cFetched), hr==S_OK )
  {
    IPropertyBag *pBag;
    hr = pM->BindToStorage(0, 0, IID_IPropertyBag, (void **)&pBag);
    if(SUCCEEDED(hr)) 
      {
      IErrorLog* errorLog=NULL;
        VARIANT var;
      VariantInit(&var);    
        var.vt = VT_BSTR;
        hr = pBag->Read(L"FriendlyName", &var, errorLog);
        if (hr == NOERROR) 
        {
          string cDevName = (char*)((_bstr_t)(var.bstrVal));
        // if the string is empty, take first device
        // if device is equal to defined string take it as well
        if(sCaptureDeviceName_ == "" ||
          sCaptureDeviceName_.compare(cDevName)==0)
        {
          cout<< "Found CaptureDevice: " << cDevName.c_str() <<endl;
          Identifier_ = cDevName;
          hr = pM->BindToObject(0, 0, IID_IBaseFilter, (void**)&pSrc);
          if(!SUCCEEDED(hr))  
            cout<< "Failed to bind to object!" <<endl;
          else
            cout<< "Successfully bound to object!" <<endl;
          break;
        }
        else
          cout<<"The defined device is not found taking next.."<<endl;    
        SysFreeString(var.bstrVal);
      }
      pBag->Release();
    }
    pM->Release();
  }
  *ppSrcFilter = pSrc;
  return hr;
}

HRESULT VideoSource_DSHOW::EnumFilters_(IFilterGraph *pGraph) 
{
    IEnumFilters *pEnum = NULL;
    IBaseFilter *pFilter;
    ULONG cFetched;

    HRESULT hr = pGraph->EnumFilters(&pEnum);
    if (FAILED(hr)) return hr;

    while(pEnum->Next(1, &pFilter, &cFetched) == S_OK)
    {
        FILTER_INFO FilterInfo;
        hr = pFilter->QueryFilterInfo(&FilterInfo);
        if (FAILED(hr))
        {
            MessageBox(NULL, TEXT("Could not get the filter info"),
                TEXT("Error"), MB_OK | MB_ICONERROR);
            continue;  // Maybe the next one will work.
        }

        // The FILTER_INFO structure holds a pointer to the Filter Graph
        // Manager, with a reference count that must be released.
        if (FilterInfo.pGraph != NULL)
        {
            FilterInfo.pGraph->Release();
        }
        if( bShowDialogs_ )
            DisplayFilterProperties( pFilter );

      /*  IPin * pPin = GetPin( pFilter, PINDIR_INPUT );
        pPin->Release();*/

        pFilter->Release();
    }

    pEnum->Release();
    return hr;
}



void VideoSource_DSHOW::InitCamControls_() 
{
  HRESULT hr = pSrcFilter_->QueryInterface(IID_IAMCameraControl, (void**)&pCameraControl_  );
  if (SUCCEEDED(hr)){
    cout <<"Have a CameraControl"<<endl;
  }
  //CameraControl_Zoom;
  ControlZoom_.id =  KSPROPERTY_CAMERACONTROL_ZOOM;
  if(pCameraControl_!=NULL)
  {
    hr = pCameraControl_->GetRange(ControlZoom_.id,&ControlZoom_.min,
        &ControlZoom_.max,&ControlZoom_.stepping,&ControlZoom_.standard,
          &ControlZoom_.flags);
    if (!SUCCEEDED(hr)) ControlZoom_.id = -1;

    ControlPan_.id =  CameraControl_Pan;
    hr = pCameraControl_->GetRange(ControlPan_.id,&ControlPan_.min,
        &ControlPan_.max,&ControlPan_.stepping,&ControlPan_.standard,
          &ControlPan_.flags);
    if (!SUCCEEDED(hr)) ControlPan_.id = -1;

    ControlTilt_.id =  CameraControl_Tilt;
    hr = pCameraControl_->GetRange(ControlTilt_.id,&ControlTilt_.min,
        &ControlTilt_.max,&ControlTilt_.stepping,&ControlTilt_.standard,
          &ControlTilt_.flags);
    if (!SUCCEEDED(hr)) ControlTilt_.id = -1;

    ControlExposure_.id =  CameraControl_Exposure;
    hr = pCameraControl_->GetRange(ControlExposure_.id,&ControlExposure_.min,
        &ControlExposure_.max,&ControlExposure_.stepping,&ControlExposure_.standard,
          &ControlExposure_.flags);
    if (!SUCCEEDED(hr)) ControlExposure_.id = -1;
    else{
      //set variables of VideoSource_Base 
      minShutter_ = ControlExposure_.min;
      maxShutter_ = ControlExposure_.max;

      cout<<"Exposure control:"<<endl;
      cout<<"min:"<<ControlExposure_.min<<endl;
      cout<<"max:"<<ControlExposure_.max<<endl;
      cout<<"stepping:"<<ControlExposure_.stepping<<endl;
      cout<<"standard:"<<ControlExposure_.standard<<endl;
    }
    ControlBrightness_.id = VideoProcAmp_Brightness;
  }

  hr = pSrcFilter_->QueryInterface(IID_IAMVideoProcAmp, (void**)&pVideoProcControl_  );
  if (SUCCEEDED(hr)){
    cout <<"Have a VideoProcAmp"<<endl;
  }
  if(pVideoProcControl_!=NULL)
  {
    hr = pVideoProcControl_->GetRange(ControlBrightness_.id,&ControlBrightness_.min,
      &ControlBrightness_.max,&ControlBrightness_.stepping,&ControlBrightness_.standard,
      &ControlBrightness_.flags);
    if (!SUCCEEDED(hr)) ControlBrightness_.id = -1;
    else{
       //set variables of VideoSource_Base 
      minGain_ = ControlBrightness_.min;
      maxGain_ = ControlBrightness_.max;
      cout<<"Brightness control:"<<endl;
      cout<<"min:"<<ControlBrightness_.min<<endl;
      cout<<"max:"<<ControlBrightness_.max<<endl;
      cout<<"stepping:"<<ControlBrightness_.stepping<<endl;
      cout<<"standard:"<<ControlBrightness_.standard<<endl;
    }
  /* Doesn't work for ICUBE Cameras
    ControlContrast_.id = VideoProcAmp_Contrast;
    ControlContrast_.standard = VideoProcAmp_Contrast;
    hr = pVideoProcControl_->GetRange(ControlContrast_.id,&ControlContrast_.min,
      &ControlContrast_.max,&ControlContrast_.stepping,&ControlContrast_.standard,
      &ControlContrast_.flags);
    if (!SUCCEEDED(hr)) ControlContrast_.id = -1;
    else{
        //set variables of VideoSource_Base 
      minContrast_ = ControlContrast_.min;
      maxContrast_ = ControlContrast_.max;
      cout<<"Contrast control:"<<endl;
      cout<<"min:"<<ControlContrast_.min<<endl;
      cout<<"max:"<<ControlContrast_.max<<endl;
      cout<<"stepping:"<<ControlContrast_.stepping<<endl;
      cout<<"standard:"<<ControlContrast_.standard<<endl;
    }*/
  }
}
// Pass it a file name in wszPath, and it will save the filter graph
// to that file.
HRESULT VideoSource_DSHOW::SaveGraphFile_(IGraphBuilder *pGraph, WCHAR *wszPath) 
{
    const WCHAR wszStreamName[] = L"ActiveMovieGraph"; 
    HRESULT hr;
    IStorage *pStorage = NULL;

    // First, create a document file that will hold the GRF file
    hr = StgCreateDocfile(
         wszPath,
         STGM_CREATE | STGM_TRANSACTED | STGM_READWRITE | 
            STGM_SHARE_EXCLUSIVE,
         0, &pStorage);
    if(FAILED(hr)) 
    {
        return hr;
    }

    // Next, create a stream to store.
    IStream *pStream;
    hr = pStorage->CreateStream(
         wszStreamName,
         STGM_WRITE | STGM_CREATE | STGM_SHARE_EXCLUSIVE,
         0, 0, &pStream);
    if (FAILED(hr)) 
    {
        pStorage->Release();    
        return hr;
    }

    // The IpersistStream::Save method converts a stream
    // into a persistent object.
    IPersistStream *pPersist = NULL;
    pGraph->QueryInterface(IID_IPersistStream, 
                           reinterpret_cast<void**>(&pPersist));
    hr = pPersist->Save(pStream, TRUE);
    pStream->Release();
    pPersist->Release();
    if (SUCCEEDED(hr)) 
    {
        hr = pStorage->Commit(STGC_DEFAULT);
    }
    pStorage->Release();
    return hr;
}


float VideoSource_DSHOW::GetShutter()
{
  if (pCameraControl_ ) {
    long prop = CameraControl_Exposure;
    long value,flags;
    HRESULT hr =  pCameraControl_->Get(prop,&value,&flags);
    //cout <<"camera shutter get: "<<value<<endl;
    //float exptime = pow(2.0f,float(value));   
    //exptime/=1000.0;
    float exptime = float(value)/10000.0f;
    return exptime;
  }
  else
    return -1;
}

int VideoSource_DSHOW::SetShutter(float exptime)
{
   if (pCameraControl_ ) {   
    exptime*=10000.0;
    long prop = CameraControl_Exposure;
    long value,flags = CameraControl_Flags_Manual;
   
    //double tmp = log(exptime) / log(2.0);
    //value = long(rint(tmp));
   
    value = long(exptime);
    //cout <<"camera shutter set: "<<value<<" [ms]"<<endl;
    HRESULT hr = pCameraControl_->Set(prop,value,flags);
   
    if (SUCCEEDED(hr)) {
      return 0;
    }
    else 
      return -1;
  }
   else return -1;
}


void VideoSource_DSHOW::SetAutoShutter(bool b)
{
   if (pCameraControl_ ) {
    long prop = CameraControl_Exposure;
    long value,flags = CameraControl_Flags_Auto;
    if(!b) flags = CameraControl_Flags_Manual;    
    value = 0;
    HRESULT hr = pCameraControl_->Set(prop,value,flags);  
  }  
}

bool VideoSource_DSHOW::GetAutoShutter()
{
  if (pCameraControl_ ) {
    long prop = CameraControl_Exposure;
    long value,flags = CameraControl_Flags_Auto;  
    value =0;
    HRESULT hr = pCameraControl_->Get(prop,&value,&flags);         
    if (SUCCEEDED(hr)) return true;
    else return false;
  }  
  return false;
}

bool VideoSource_DSHOW::
HasControlShutter(){
  if(pCameraControl_!= NULL)
    return true;
  else 
    return false;
}

bool VideoSource_DSHOW::HasControlBrightness()
{
  if (pVideoProcControl_ && ControlBrightness_.id >=0) return true;
  return false;
}


float VideoSource_DSHOW::GetBrightness()
{
  if (pVideoProcControl_ && ControlBrightness_.id >=0) {
    long value,flags;
    pVideoProcControl_->Get(ControlBrightness_.id,&value,&flags);
    //cout <<"camera gain get: "<<value<<" [dB]"<<endl;
    float br = float(value);
    //br /=100.0f;
    return br;
 }
  return 0;
}

int VideoSource_DSHOW::SetBrightness(float g)
{
  if (pVideoProcControl_ && ControlBrightness_.id >=0) {
    // g*=100.0f;
    long value = long(g);
    //cout <<"camera gain set: "<<value<<" [dB]"<<endl;
    pVideoProcControl_->Set(ControlBrightness_.id,value,ControlBrightness_.flags);
    return 0;
 }else return -1;
  return 0;
}

void VideoSource_DSHOW::SetAutoBrightness(bool b)
{
  long value,flags;
    pVideoProcControl_->Get(ControlBrightness_.id,&value,&flags);
    if (b ) 
      flags = VideoProcAmp_Flags_Auto;
    else 
      flags = VideoProcAmp_Flags_Manual;
    pVideoProcControl_->Set(ControlBrightness_.id,value,flags);
}

bool VideoSource_DSHOW::GetAutoBrightness()
{
  long value;
  pVideoProcControl_->Get(ControlBrightness_.id,&value,&ControlBrightness_.flags);
  return ControlBrightness_.flags == VideoProcAmp_Flags_Auto;
}

bool VideoSource_DSHOW::HasControlContrast()
{
  if (pVideoProcControl_ && ControlContrast_.id >=0) return true;
  return false;
}

float VideoSource_DSHOW::GetContrast()
{
   if (pVideoProcControl_  && ControlContrast_.id >=0) {
    long value;
    pVideoProcControl_->Get(ControlContrast_.id,&value,&ControlContrast_.flags);
    float br = (float(value-ControlContrast_.min) / 
      float(ControlContrast_.max-ControlContrast_.min))* 100.0f;
    return br;
 }
  return 0;
}

int VideoSource_DSHOW::SetContrast(float g)
{
  if (pVideoProcControl_ && ControlContrast_.id >=0) {
    long value;
    float br = (g/100.f) * float(ControlContrast_.max-ControlContrast_.min) 
      + float(ControlContrast_.min);
    value = (int)rint(br);
    pVideoProcControl_->Set(ControlContrast_.id,value,ControlContrast_.flags);
    return (int)rint(br);
 }
  return 0;
}


void VideoSource_DSHOW::SetAutoContrast(bool b)
{
   long value,flags;
    pVideoProcControl_->Get(ControlContrast_.id,&value,&flags);
    if (b ) 
      flags = VideoProcAmp_Flags_Auto;
    else 
      flags = VideoProcAmp_Flags_Manual;
    pVideoProcControl_->Set(ControlContrast_.id,value,flags);
}

bool VideoSource_DSHOW::GetAutoContrast()
{
   long value;
  pVideoProcControl_->Get(ControlContrast_.id,&value,&ControlContrast_.flags);
  return ControlContrast_.flags == VideoProcAmp_Flags_Auto;
  return 0;
}




void VideoSource_DSHOW::
ResetPanTiltZoom()
{

}



////////////////////////////////////////////////////////////////////////   

//constructor
MySampleGrabberCB::MySampleGrabberCB(int width, int height, int depth)
{ 
  sampleLength_ = 1000;
  if(sampleLength_ > width*height/2) sampleLength_ = width*height/2;
  lastMemorySample_ = new unsigned char[sampleLength_];
  currentMemorySample_ = new unsigned char[sampleLength_];
  bCheckNewImage_ = false;
  if(!(camera_.GetImageData()==NULL))
    camera_.Release();
  camera_.Init(width,height,depth);
}

/****************************************************************/

MySampleGrabberCB::~MySampleGrabberCB() {
  if(!(camera_.GetImageData()==NULL))
    camera_.Release();
  delete[] lastMemorySample_;
  delete[] currentMemorySample_;
}
/****************************************************************/
STDMETHODIMP MySampleGrabberCB::BufferCB(double SampleTime, BYTE *pBuffer, long BufferLen)
{
  #ifdef BIAS_HAVE_PTHREADS
  //after wakeup lock again %
  pthread_mutex_lock(&imageMutex_); 
  #endif
  unsigned char *pImageData = camera_.GetImageData();
  BIASASSERT(BufferLen==(long)camera_.GetSizeByte());
  memcpy(pImageData,pBuffer,BufferLen);

  //reset pImageData to beginning of image
  pImageData = camera_.GetImageData()+(camera_.GetSizeByte()/2);
  //copy image data to current sample data
  memcpy(currentMemorySample_,pImageData,sampleLength_);
  
  if (UpSideDown_) camera_.Flip();
  #ifdef BIAS_HAVE_PTHREADS
  //after wakeup lock again %
  pthread_mutex_unlock(&imageMutex_); 
  #endif
  return S_OK;
}

/****************************************************************/
STDMETHODIMP MySampleGrabberCB::SampleCB(double SampleTime, IMediaSample *pSample)
{
  #ifdef BIAS_HAVE_PTHREADS
  pthread_mutex_lock(&imageMutex_);
  #endif

  //set pImageData to beginning of image
  unsigned char *pImageData = camera_.GetImageData();
  BYTE *pSampleData;
  pSample->GetPointer(&pSampleData);
  long size = pSample->GetSize();
  BIASASSERT(size==(long)camera_.GetSizeByte());
  //copy sampleData to imageData of BIAS image
  memcpy(pImageData,pSampleData,size);
    
  //set pointer to middle of image for memory comparison
  pImageData = camera_.GetImageData()+(camera_.GetSizeByte()/2);;
  //copy image data to current memory sample data
  memcpy(currentMemorySample_,pImageData,sampleLength_);
 
  if (UpSideDown_) camera_.Flip();
  #ifdef BIAS_HAVE_PTHREADS
  pthread_mutex_unlock(&imageMutex_);
  #endif
  return S_OK;
}

/****************************************************************/

STDMETHODIMP MySampleGrabberCB::QueryInterface(REFIID riid, void ** ppv) {

    if( riid == IID_ISampleGrabberCB || riid == IID_IUnknown ) {
        *ppv = (void *) static_cast<ISampleGrabberCB*> ( this );
        return NOERROR;
    }    
    return E_NOINTERFACE;
}

bool MySampleGrabberCB::
HasNewImage()
{
  if(bCheckNewImage_){
    //check if they are identical
    if(memcmp(lastMemorySample_,currentMemorySample_,sampleLength_)==0){
      //cout<<"old";
      return false;
    }
    else  {
      //if they are different back up old
      memcpy(lastMemorySample_,currentMemorySample_,sampleLength_);
      //cout<<"new"<<endl;
      return true;
    }
  }
  else 
    return true;
}