/// purpose:     Definition of the Camera1394 class
///
/// @date created 2011/02/28 - 15:21
/// @author Sergio Rodriguez
/// @copyright Heudiasyc UMR UTC/CNRS 6599
/// @version:     $Id$

#include "Camera1394Unix.h"

#include "structure/genericStructures.h"
#include <kernel/inputOutputInterface.h>
//#include <qimage.h>
#include <qimagewriter.h>
#include <qmutex.h>

#include "kernel/ComponentFactory.h"
#include "kernel/DbiteFileTypes.h"
#include "kernel/Log.h"
#include "PacpusTools/ShMem.h"

namespace pacpus {

using namespace std;

DECLARE_STATIC_LOGGER("pacpus.base.Camera1394");

// Construct the factory
ComponentFactory<Camera1394> sFactory("Camera1394");

void Camera1394::addInputOutput()
{
    output.insert("image",new OutputInterface<PacpusImage,Camera1394> ("image",this));
}

//////////////////////////////////////////////////////////////////////////
// Contructor
//////////////////////////////////////////////////////////////////////////
Camera1394::Camera1394(QString name)
    : ComponentBase(name)
{
  qimage_ = NULL; 
  acquiring_ = FALSE;
  recording_ = FALSE;

  width_  = 0;
  height_ = 0;

  tr_ = 0; 
  addInputOutput();
}

//////////////////////////////////////////////////////////////////////////
// Destructor
//////////////////////////////////////////////////////////////////////////
Camera1394::~Camera1394()
{
  // verify that the camera has been initialized
  //if (theCamera->m_cameraInitialized)
    //if (theCamera->StopImageAcquisition() != CAM_SUCCESS)
    //  qFatal("Problem Stopping Image Acquisition");

  delete qimage_;
  qimage_ = NULL;
}

//////////////////////////////////////////////////////////////////////////
// Configure the component 
// Parameters are: 
// - node: the number of the camera in the driver list. Normally, node 0 
// is for the first camera connected, node is incremented for other 
//////////////////////////////////////////////////////////////////////////
ComponentBase::COMPONENT_CONFIGURATION Camera1394::configureComponent(XmlComponentConfig config)
{
  cameraNode_ = param.getProperty("node").toInt();
  videoFormat_ = param.getProperty("format").toInt();
  videoMode_ = param.getProperty("mode").toInt();
  frameRate_ = param.getProperty("framerate").toInt();
  recording_ = (param.getProperty("recording") == "true" ? true : false );
  recordFormat_ = param.getProperty("recordFormat");
  reverseImage_ = (param.getProperty("reverseImage") == "true" ? true : false);
  displaying_ = (param.getProperty("display") == "true" ? true : false);
  zoom_ = param.getProperty("zoom").toInt();

  imageMutex_ = new QMutex;

  if (displaying_)
  {
    viewer_ = new ImageViewer;
    viewer_->setMutex(imageMutex_);
    viewer_->show();
    viewer_->setWindowTitle(componentName);
    connect(this, SIGNAL( newImage(QImage*) ), viewer_, SLOT( display(QImage*) ) );
  }

  switch(frameRate_){
  case 0:
      theFramerate = DC1394_FRAMERATE_1_875;
      break;
  case 1:
      theFramerate = DC1394_FRAMERATE_3_75;
      break;
  case 2:
      theFramerate = DC1394_FRAMERATE_7_5;
      break;
  case 3:
      theFramerate = DC1394_FRAMERATE_15;
      break;
  case 4:
      theFramerate = DC1394_FRAMERATE_30;
      break;
  case 5:
      theFramerate = DC1394_FRAMERATE_60;
      break;
  default:
      qFatal("Framerate not supported");
      break;
  }

  return ComponentBase::CONFIGURED_OK;
}

//////////////////////////////////////////////////////////////////////////
// Stop the acquisition of data 
//////////////////////////////////////////////////////////////////////////
void Camera1394::stopActivity()
{
    stopAcquiring();

    // wait the thread termination
    if (!wait(3000)) {
        terminate();
        LOG_WARN(componentName << ": The thread doesn't respond for 1 second, it has been terminated");
    }
    if (recording_) {
        file_.close();
    }
}

//////////////////////////////////////////////////////////////////////////
// Start the camera 
// Set all video parameters (resolution, mode and framerate) 
//////////////////////////////////////////////////////////////////////////
void Camera1394::startActivity()
{
    dc1394error_t err;

    if (recording_) {
        dbtFileName_ = componentName + ".dbt";
        file_.open(dbtFileName_.toStdString(), WriteMode, FILE_JPEG, MAX_CHAR_DIRECTORY_NAME+MAX_CHAR_PICTURE_NAME);
    }
    if(!initCamera())
        return;

    // make the calls to set up the capture mode
    dc1394_video_set_iso_speed( theCamera, DC1394_ISO_SPEED_400 );
    dc1394_video_set_mode(      theCamera, theCamera_video_mode );
    dc1394_video_set_framerate( theCamera, theFramerate);

    err = dc1394_capture_setup( theCamera, 8, DC1394_CAPTURE_FLAGS_DEFAULT );
    if ( err != DC1394_SUCCESS )
    {
        cout << "Can't setup capture" << endl;
        return;
    }
    // have the camera start sending us data
    err = dc1394_video_set_transmission( theCamera, DC1394_ON );
    if ( err != DC1394_SUCCESS )
    {
        fprintf( stderr, "Unable to start camera iso transmission\n" );
        return;
    }
    printf( "Waiting for transmission... \n" );
    //  Sleep untill the camera has a transmission
    dc1394switch_t status = DC1394_OFF;

    for ( int i = 0; i <= 5; i++ )
    {
        usleep(50000);
        err = dc1394_video_get_transmission( theCamera, &status );
        if ( err != DC1394_SUCCESS )
        {
            fprintf( stderr, "Unable to get transmision status\n" );
            return;
        }
        if ( status != DC1394_OFF )
            break;

        if( i == 5 )
        {
            fprintf(stderr,"Camera doesn't seem to want to turn on!\n");
            return;
        }
    }

//  // switch some controls in Automatic mode
//  theCamera->m_controlWhiteBalance.SetAutoMode(TRUE);
//  theCamera->m_controlShutter.SetAutoMode(TRUE);
//  theCamera->m_controlGain.SetAutoMode(TRUE);
//  theCamera->m_controlIris.SetAutoMode(TRUE);
//
//  theCamera->SetZoom(zoom_);

    // alloc images
    if (!qimage_)
        qimage_ = new QImage( width_, height_, QImage::Format_RGB32 );

    startAcquiring();
    start();
}

//////////////////////////////////////////////////////////////////////////
// Initialize the C1394Camera Driver 
// Verify the presence of a compatible camera and select it 
// If it doesn't work, please have a look on the Windows driver in the 
// hardware manager 
//////////////////////////////////////////////////////////////////////////
bool Camera1394::initCamera()
{
    dc1394error_t err;
    dc1394_t * d;
    dc1394camera_list_t * list;

    dc1394video_modes_t 	video_modes;
    dc1394video_mode_t 	  required_video_mode;
    dc1394color_coding_t 	coding;

    bool printSupportedFormats = false;
    bool foundFormatVideoMode = false;

    // Find cameras on the 1394 buses
    d = dc1394_new ();

    // Enumerate cameras connected to the PC
    err = dc1394_camera_enumerate (d, &list);
    if ( err != DC1394_SUCCESS )
    {
        fprintf( stderr, "Unable to look for cameras\n\n"
                 "Please check \n"
                 "  - if the kernel modules `ieee1394',`raw1394' and `ohci1394' "
                 "are loaded \n"
                 "  - if you have read/write access to /dev/raw1394\n\n");
        return false;
    }

    if (list->num == 0)
    {
        cout << "No cameras found!" << endl;
        return false;
    }

    cout << "There is(are)" << list->num <<" camera(s) connected"<< endl;
    if(list->num>cameraNode_){
        theCamera = dc1394_camera_new(d, list->ids[cameraNode_].guid);
        if (!theCamera){
            LOG_ERROR("Failed to initialize camera with guid " << list->ids[cameraNode_].guid);
        } else {
            LOG_INFO("Camera model = '"<< theCamera->model << " was initialized");
        }
    }

    err = dc1394_video_get_supported_modes( theCamera, &video_modes );
    if ( err != DC1394_SUCCESS )
    {
        fprintf( stderr, "Can't get video modes\n" );
        return err;
    }

    //Camera video mode selection
    switch(videoFormat_){
    case 0:
        switch(videoMode_){
        case 0:
            required_video_mode = DC1394_VIDEO_MODE_160x120_YUV444;
            width_  = 160;
            height_ = 120;
            break;
        case 1:
            required_video_mode = DC1394_VIDEO_MODE_320x240_YUV422;
            width_  = 320;
            height_ = 240;
            break;
        case 2:
            required_video_mode = DC1394_VIDEO_MODE_640x480_YUV411;
            width_  = 640;
            height_ = 480;
            break;
        case 3:
            required_video_mode = DC1394_VIDEO_MODE_640x480_YUV422;
            width_  = 640;
            height_ = 480;
            break;
        case 4:
            required_video_mode = DC1394_VIDEO_MODE_640x480_RGB8;
            width_  = 640;
            height_ = 480;
            break;
        case 5:
            required_video_mode = DC1394_VIDEO_MODE_640x480_MONO8;
            width_  = 640;
            height_ = 480;
            break;
        case 6:
            required_video_mode = DC1394_VIDEO_MODE_640x480_MONO16;
            width_  = 640;
            height_ = 480;
            break;
        default:
            cout << "The selected video mode is not supported or available"<< endl;
            return false;
            break;
        }
        break;
    case 1:
        switch(videoMode_){
        case 0:
            required_video_mode = DC1394_VIDEO_MODE_800x600_YUV422;
            width_  = 800;
            height_ = 600;
            break;
        case 1:
            required_video_mode = DC1394_VIDEO_MODE_800x600_RGB8;
            width_  = 800;
            height_ = 600;
            break;
        case 2:
            required_video_mode = DC1394_VIDEO_MODE_800x600_MONO8;
            width_  = 800;
            height_ = 600;
            break;
        case 3:
            required_video_mode = DC1394_VIDEO_MODE_1024x768_YUV422;
            width_  = 1024;
            height_ = 768;
            break;
        case 4:
            required_video_mode = DC1394_VIDEO_MODE_1024x768_RGB8;
            width_  = 1024;
            height_ = 768;
            break;
        case 5:
            required_video_mode = DC1394_VIDEO_MODE_1024x768_MONO8;
            width_  = 1024;
            height_ = 768;
            break;
        case 6:
            required_video_mode = DC1394_VIDEO_MODE_800x600_MONO16;
            width_  = 800;
            height_ = 600;
            break;
        case 7:
            required_video_mode = DC1394_VIDEO_MODE_1024x768_MONO16;
            width_  = 1024;
            height_ = 768;
            break;
        default:
            cout << "The selected video mode is not supported or available"<< endl;
            printSupportedFormats =true;
            break;
        }
        break;
    case 2:
        switch(videoMode_){
        case 0:
            required_video_mode = DC1394_VIDEO_MODE_1280x960_YUV422;
            width_  = 1280;
            height_ = 960;
            break;
        case 1:
            required_video_mode = DC1394_VIDEO_MODE_1280x960_RGB8;
            width_  = 1280;
            height_ = 960;
            break;
        case 2:
            required_video_mode = DC1394_VIDEO_MODE_1280x960_MONO8;
            width_  = 1280;
            height_ = 960;
            break;
        case 6:
            required_video_mode = DC1394_VIDEO_MODE_1280x960_MONO16;
            width_  = 1280;
            height_ = 960;
            break;
        default:
            cout << "The selected video mode is not supported or available"<< endl;
            printSupportedFormats =true;
            break;
        }
        break;
    default:
        cout << "The selected format is not supported or available"<< endl;
        printSupportedFormats =true;
        break;
    }

    //looking for selected mode on camera
    for ( int i = video_modes.num-1; i >= 0; i-- )
    {
        // don't consider FORMAT 7 modes (i.e. "scalable")
        if ( !dc1394_is_video_mode_scalable( video_modes.modes[i] ) )
        {
            dc1394_get_color_coding_from_video_mode( theCamera, video_modes.modes[i], &coding );
            if(required_video_mode==video_modes.modes[i]){
                foundFormatVideoMode=true;
                theCamera_video_mode=required_video_mode;
                cout << "Correct format/video mode." << endl;
            }
        }
    }
    if (!foundFormatVideoMode)
        printSupportedFormats=true;

    if (printSupportedFormats){
        cout << "Error, only the followng format/video modes are supported: " << endl;
        //Supported modes
        for ( int i = video_modes.num-1; i >= 0; i-- )
        {
            // don't consider FORMAT 7 modes (i.e. "scalable")
            if ( !dc1394_is_video_mode_scalable( video_modes.modes[i] ) )
            {
                dc1394_get_color_coding_from_video_mode( theCamera, video_modes.modes[i], &coding );

                switch(video_modes.modes[i])
                {
                case (DC1394_VIDEO_MODE_160x120_YUV444):
                    cout << "DC1394_VIDEO_MODE_160x120_YUV444" << endl;
                    break;
                case (DC1394_VIDEO_MODE_320x240_YUV422):
                    cout << "DC1394_VIDEO_MODE_320x240_YUV422 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_640x480_YUV411):
                    cout << "DC1394_VIDEO_MODE_640x480_YUV411 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_640x480_YUV422):
                    cout << "DC1394_VIDEO_MODE_640x480_YUV422 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_640x480_RGB8):
                    cout << "DC1394_VIDEO_MODE_640x480_RGB8 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_640x480_MONO8):
                    cout << "DC1394_VIDEO_MODE_640x480_MONO8 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_640x480_MONO16):
                    cout << "DC1394_VIDEO_MODE_640x480_MONO16 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_800x600_YUV422):
                    cout << "DC1394_VIDEO_MODE_800x600_YUV422 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_800x600_RGB8):
                    cout << "DC1394_VIDEO_MODE_800x600_RGB8 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_800x600_MONO8):
                    cout << "DC1394_VIDEO_MODE_800x600_MONO8 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_1024x768_YUV422):
                    cout << "DC1394_VIDEO_MODE_1024x768_YUV422 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_1024x768_RGB8):
                    cout << "DC1394_VIDEO_MODE_1024x768_RGB8 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_1024x768_MONO8):
                    cout << "DC1394_VIDEO_MODE_1024x768_MONO8 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_800x600_MONO16):
                    cout << "DC1394_VIDEO_MODE_800x600_MONO16 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_1024x768_MONO16):
                    cout << "DC1394_VIDEO_MODE_1024x768_MONO16 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_1280x960_YUV422):
                    cout << "DC1394_VIDEO_MODE_1280x960_YUV422 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_1280x960_RGB8):
                    cout << "DC1394_VIDEO_MODE_1280x960_RGB8 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_1280x960_MONO8):
                    cout << "DC1394_VIDEO_MODE_1280x960_MONO8 " << endl;
                    break;
                case (DC1394_VIDEO_MODE_1280x960_MONO16):
                    cout << "DC1394_VIDEO_MODE_1280x960_MONO16 " << endl;
                    break;

                }
            }

        }
        return false;
    }

    if(recording_){
        char commandline[64];
        sprintf(commandline,"mkdir %s\n",componentName.toLatin1().data());
        system(commandline);
        puts(commandline);
    }

    // FIXME: uncomplete type of d: dc1394_t
    delete d;
    delete list;
    return true;
}

bool Camera1394::recordImage(QString format)
{      
    imageCounter_=imageCounter_+1;
    bool formatSupported = false;

    format = format.toLower();

    if (format == "jpeg")
        format = "jpg";

    QImageWriter imageWriter;
    if( imageWriter.supportedImageFormats().contains(format.toLatin1()) )
        formatSupported = true;

    if (formatSupported == false) {
        LOG_ERROR(componentName << "Format" << format << "not supported for recording");
    } else {
        // write in the dbt file
        stringstream imageNameSs;
        imageNameSs << componentName.toStdString();
        imageNameSs << "/" << imageCounter_ << "." << format.toStdString();
        const char * imageName = imageNameSs.str().c_str();
        imageWriter.setFileName(imageName);
        imageWriter.setFormat( format.toLatin1() );
        imageWriter.write(*qimage_);
        file_.writeRecord(time_, tr_, imageName, MAX_CHAR_DIRECTORY_NAME+MAX_CHAR_PICTURE_NAME);
    }
    return formatSupported;
}

//#define MEASURE_TIME

//////////////////////////////////////////////////////////////////////////
// Main loop for acquisition
//////////////////////////////////////////////////////////////////////////
void Camera1394::run()
{ 
  dc1394error_t err;

  bool firstTime = true;
  ShMem * shMem;
  imageCounter_ = 0;
  time_ = road_time(); 

  road_time_t t; 
  
  while (acquiring_)
  {
      err = dc1394_capture_dequeue( theCamera, DC1394_CAPTURE_POLICY_WAIT, &theFrame );//DC1394_CAPTURE_POLICY_POLL
      if ( err != DC1394_SUCCESS ){
          fprintf( stderr, "Camera acquisition: Cannot dequeue image!\n" );
          return;
      }

      prevTime_ = time_;
      time_ = road_time();

      // verify that the acquisition period is good relatively to the selected format
      if ( isPeriodGood((int)(time_ - prevTime_)) )
          setState(MONITOR_OK);
      else
          setState(MONITOR_NOK);

      t = road_time();
      if ( !fillRGB32Image(qimage_->bits()) ) continue;

      // return buffer for use
      dc1394_capture_enqueue( theCamera, theFrame );

#ifdef MEASURE_TIME
      LOG_INFO(componentName << "duration conversion: " << static_cast<int64_t>(road_time() - t) ;
#endif 

      imageMutex_->lock();

      t = road_time();
      if (reverseImage_)
          *qimage_ = qimage_->mirrored(true,true);

      // send image in shared memory
      if (firstTime)
      {
          shMem = new ShMem("IMAGE", qimage_->byteCount());
          firstTime = false;
      }
      shMem->write(qimage_->bits(), qimage_->byteCount());

#ifdef MEASURE_TIME
      LOG_INFO(componentName << "duration mirror: " << static_cast<int64_t>(road_time() - t) ;
#endif 

      imageMutex_->unlock();
      emit newImage(qimage_);

      t = road_time();
      PacpusImage data;
      data.time = t;
      data.image = *qimage_;
      OutputInterface<PacpusImage,Camera1394> * out = static_cast<OutputInterface<PacpusImage,Camera1394> *> (output.value("image"));
      out->send(data);

#ifdef MEASURE_TIME
      LOG_INFO(componentName << "duration mirror: " << static_cast<int64_t>(road_time() - t) ;
#endif

      t = road_time();
      if (recording_)
      {
          recordImage(recordFormat_);
      }
#ifdef MEASURE_TIME
      LOG_INFO(componentName << "duration record: " << static_cast<int64_t>(road_time() - t));
#endif

  } // END while (acquiring_)

  LOG_INFO(componentName << ":Stopping the camera");

  dc1394_capture_stop(theCamera);
  dc1394_video_set_transmission(theCamera,DC1394_OFF);
  dc1394_camera_free(theCamera);

  //if (theCamera->StopImageAcquisition() != CAM_SUCCESS)
  //  qFatal("Problem Stopping Image Acquisition");

  if (!firstTime)
      delete shMem;
  setState(STOPPED); 
}

//////////////////////////////////////////////////////////////////////////
// return the actual period in µs corresponding to the framerate code
//////////////////////////////////////////////////////////////////////////
double Camera1394::getPeriod()
{
  switch(frameRate_) {
  case 0:
    return 1.0e6/1.875; 
    break;
    
  case 1:
    return 1.0e6/3.75; 
    break;
    
  case 2:
    return 1.0e6/7.5; 
    break;
    
  case 3:
    return 1.0e6/15; 
    break;
    
  case 4:
    return 1.0e6/30; 
    break;
    
  case 5:
    return 1.0e6/60; 
    break;
    
  default:
    LOG_WARN("Unknown framerate. The component " << componentName << " may encounter a problem !");
    return 0; 
  }
}

//////////////////////////////////////////////////////////////////////////
// retrn true if the period T corresponds to the framerate (at +/-20%)
// T is given in microseconds
//////////////////////////////////////////////////////////////////////////
bool Camera1394::isPeriodGood(int T)
{
  float period = getPeriod();  
  if ( (T < period*0.8) || (T > period*1.2) )
    return false; 
  else 
    return true; 
}

//////////////////////////////////////////////////////////////////////////
// useful macro to keep 'a' between 0 and 255
//////////////////////////////////////////////////////////////////////////
#define CLAMP_TO_UCHAR(a) (unsigned char)((a) < 0 ? 0 : ((a) > 255 ? 255 : (a)))

//////////////////////////////////////////////////////////////////////////
// Convert the image from YUV444 to RGB32
// The image is stored using a 32-bit RGB format (0xffRRGGBB)
//////////////////////////////////////////////////////////////////////////
void Camera1394::YUV444toRGB32(unsigned char* buffer)
{
	long Y, U, V, deltaG;
	unsigned char *srcptr, *srcend, *destptr;

	// single-stage idiotproofing
//	if(theCamera->m_pData == NULL || buffer == NULL)
//		return;
	
  srcptr = theFrame->image;
  srcend = srcptr + (width_ * height_ * 3);
	destptr = buffer;

	// data pattern: UYV
	// unroll it to 4 pixels/round

	while(srcptr < srcend)
	{
		U = (*srcptr++) - 128;
		Y = (*srcptr++);
		V = (*srcptr++) - 128;

		deltaG = (12727 * U + 33384 * V);
		deltaG += (deltaG > 0 ? 32768 : -32768);
		deltaG >>= 16;
    
    *destptr++ = CLAMP_TO_UCHAR( Y + U );
		*destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
		*destptr++ = CLAMP_TO_UCHAR( Y + V );
    destptr++;//A

		U = (*srcptr++) - 128;
		Y = (*srcptr++);
		V = (*srcptr++) - 128;

		deltaG = (12727 * U + 33384 * V);
		deltaG += (deltaG > 0 ? 32768 : -32768);
		deltaG >>= 16;

    *destptr++ = CLAMP_TO_UCHAR( Y + U );
		*destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
		*destptr++ = CLAMP_TO_UCHAR( Y + V );
    destptr++;//A

		U = (*srcptr++) - 128;
		Y = (*srcptr++);
		V = (*srcptr++) - 128;

		deltaG = (12727 * U + 33384 * V);
		deltaG += (deltaG > 0 ? 32768 : -32768);
		deltaG >>= 16;

    *destptr++ = CLAMP_TO_UCHAR( Y + U );
		*destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
		*destptr++ = CLAMP_TO_UCHAR( Y + V );
    destptr++;//A

		U = (*srcptr++) - 128;
		Y = (*srcptr++);
		V = (*srcptr++) - 128;

		deltaG = (12727 * U + 33384 * V);
		deltaG += (deltaG > 0 ? 32768 : -32768);
		deltaG >>= 16;

    *destptr++ = CLAMP_TO_UCHAR( Y + U );
		*destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
		*destptr++ = CLAMP_TO_UCHAR( Y + V );
    destptr++;//A
	}
}

//////////////////////////////////////////////////////////////////////////
// Convert the image from YUV422 to RGB32
// The image is stored using a 32-bit RGB format (0xffRRGGBB)
//////////////////////////////////////////////////////////////////////////
void Camera1394::YUV422toRGB32(unsigned char *buffer)
{
  long Y, U, V, deltaG;
  unsigned char *srcptr, *srcend, *destptr;
  
  srcptr = theFrame->image;
  srcend = srcptr + ((width_ * height_) << 1);
  destptr = buffer;
  
  // single-stage idiotproofing
//  if(theCamera->m_pData == NULL || buffer == NULL)
//  {
//    return;
//  }
  
  // data pattern: UYVY
  
  while(srcptr < srcend)
  {
    U = *srcptr;
    U -= 128;
    V = *(srcptr+2);
    V -= 128;
    
    deltaG = (12727 * U + 33384 * V);
    deltaG += (deltaG > 0 ? 32768 : -32768);
    deltaG >>= 16;
    
    Y = *(srcptr + 1);
    *destptr++ = CLAMP_TO_UCHAR( Y + U );
    *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
    *destptr++ = CLAMP_TO_UCHAR( Y + V );
    destptr++;//A
    
    
    Y = *(srcptr + 3);
    *destptr++ = CLAMP_TO_UCHAR( Y + U );
    *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
    *destptr++ = CLAMP_TO_UCHAR( Y + V );
    destptr++;//A
    
    srcptr += 4;
    
    // twice in the same loop... just like halving the loop overhead
    
    U = (*srcptr) - 128;
    V = (*(srcptr+2)) - 128;
    
    deltaG = (12727 * U + 33384 * V);
    deltaG += (deltaG > 0 ? 32768 : -32768);
    deltaG >>= 16;
    
    Y = *(srcptr + 1);
    *destptr++ = CLAMP_TO_UCHAR( Y + U );
    *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
    *destptr++ = CLAMP_TO_UCHAR( Y + V );
    destptr++;//A
    
    Y = *(srcptr + 3);
    *destptr++ = CLAMP_TO_UCHAR( Y + U );
    *destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
    *destptr++ = CLAMP_TO_UCHAR( Y + V );
    destptr++;//A
    
    srcptr += 4;
    
  }
}

//////////////////////////////////////////////////////////////////////////
// Convert the image from YUV411 to RGB32
// The image is stored using a 32-bit RGB format (0xffRRGGBB)
//////////////////////////////////////////////////////////////////////////
void Camera1394::YUV411toRGB32(unsigned char *buffer)
{
	long Y, U, V, deltaG;
	unsigned char *srcptr, *srcend, *destptr;

	// single-stage idiotproofing
//	if(theCamera->m_pData == NULL || buffer == NULL)
//		return;

	// data pattern: UYYVYY

  srcptr = theFrame->image;
  srcend = srcptr + ((width_ * height_ * 3) >> 1);
	destptr = buffer;

	while(srcptr < srcend)
	{
		U = (*srcptr) - 128;
		V = (*(srcptr+3)) - 128;

		deltaG = (12727 * U + 33384 * V);
		deltaG += (deltaG > 0 ? 32768 : -32768);
		deltaG >>= 16;

		Y = *(srcptr + 1);
    *destptr++ = CLAMP_TO_UCHAR( Y + U );
		*destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
		*destptr++ = CLAMP_TO_UCHAR( Y + V );
    destptr++;//A

		Y = *(srcptr + 2);
    *destptr++ = CLAMP_TO_UCHAR( Y + U );
		*destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
		*destptr++ = CLAMP_TO_UCHAR( Y + V );
    destptr++;//A

		Y = *(srcptr + 4);
    *destptr++ = CLAMP_TO_UCHAR( Y + U );
		*destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
		*destptr++ = CLAMP_TO_UCHAR( Y + V );
    destptr++;//A

		Y = *(srcptr + 5);
    *destptr++ = CLAMP_TO_UCHAR( Y + U );
		*destptr++ = CLAMP_TO_UCHAR( Y - deltaG );
		*destptr++ = CLAMP_TO_UCHAR( Y + V );
    destptr++;//A

		srcptr += 6;
	}
}

//////////////////////////////////////////////////////////////////////////
// Convert the image from Y to RGB32
// The image is stored using a 32-bit RGB format (0xffRRGGBB)
//////////////////////////////////////////////////////////////////////////
void Camera1394::YtoRGB32(unsigned char * buffer)
{
	unsigned char *srcptr, *srcend, *destptr;

  srcptr = theFrame->image;
  srcend = srcptr + (width_ * height_);
	destptr = buffer;

	// single-stage idiotproofing
//	if(theCamera->m_pData == NULL || buffer == NULL)
//		return;

	// just Y's (monochrome)

	// unroll it to 4 per cycle

	while(srcptr < srcend)
	{
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
    destptr++;//A
    srcptr++;

		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
    destptr++;//A
    srcptr++;

		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;    
    destptr++;//A
		srcptr++;

		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
    destptr++;//A
		srcptr++;
	}
}

//////////////////////////////////////////////////////////////////////////
// Convert the image from Y16 to RGB32
// The image is stored using a 32-bit RGB format (0xffRRGGBB)
// 16-bit monochrome (new to spec 1.3)
// the first of each pair of bytes is the high byte, so just copy those to RGB
//////////////////////////////////////////////////////////////////////////
void Camera1394::Y16toRGB32(unsigned char *buffer)
{
	unsigned char *srcptr, *srcend, *destptr;

  srcptr = theFrame->image;
  srcend = srcptr + 2 * (width_ * height_);
	destptr = buffer;

	// single-stage idiotproofing
//	if(theCamera->m_pData == NULL || buffer == NULL)
//		return;

	// just Y's (monochrome, 16-bit little endian)

	// unroll it to 4 per cycle

	while(srcptr < srcend)
	{
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
    destptr++;//A
		srcptr += 2;

		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
    destptr++;//A
		srcptr += 2;

		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
    destptr++;//A
		srcptr += 2;

		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
    destptr++;//A
		srcptr += 2;
	}
}

//////////////////////////////////////////////////////////////////////////
// Convert the image from RGB16 to RGB32
// The image is stored using a 32-bit RGB format (0xffRRGGBB)
// 16-bit RGB (new to spec 1.3)
// the first of each pair of bytes is the high byte, so just copy those to RGB
// near duplicate of Y16toRGB
//////////////////////////////////////////////////////////////////////////
void Camera1394::RGB16toRGB32(unsigned char * buffer)
{
	unsigned char *srcptr, *srcend, *destptr;

  srcptr = theFrame->image;
  srcend = srcptr + 6 * (width_ * height_);
	destptr = buffer;

	// single-stage idiotproofing
//	if(theCamera->m_pData == NULL || buffer == NULL)
//		return;

	// R,G,B are 16-bit source, chop of the top 8 and feed 

	// unroll it to 3 per cycle

	while(srcptr < srcend)
	{
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
    destptr++;//A
		srcptr += 2;

		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
    destptr++;//A
		srcptr += 2;

		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
		*destptr++ = *srcptr;
    destptr++;//A
		srcptr += 2;
	}
}



//////////////////////////////////////////////////////////////////////////
// Convert the image from RGB24 to RGB32
// The image is stored using a 32-bit RGB format (0xffRRGGBB)
//////////////////////////////////////////////////////////////////////////
void Camera1394::RGB24toRGB32(unsigned char * buffer)
{
	unsigned char *srcptr, *srcend, *destptr;

  srcptr = theFrame->image;
  srcend = srcptr + 3 * (width_ * height_);
	destptr = buffer;

	// single-stage idiotproofing
//	if(theCamera->m_pData == NULL || buffer == NULL)
//		return;

	// unroll it to 3 per cycle

	while(srcptr < srcend)
	{
		*destptr++ = *(srcptr+2); //B
		*destptr++ = *(srcptr+1); //G
		*destptr++ = *srcptr; //R
    destptr++;//A
    srcptr += 3;
		
		*destptr++ = *(srcptr+2);
		*destptr++ = *(srcptr+1);
		*destptr++ = *srcptr;
    destptr++;//A
    srcptr += 3;
		
		*destptr++ = *(srcptr+2);
		*destptr++ = *(srcptr+1);
		*destptr++ = *srcptr;
    destptr++;//A
    srcptr += 3;
  }
}


bool Camera1394::fillRGB32Image(unsigned char * buffer)
{
	if(buffer == NULL)
		return false;

  bool retValue = true;

	switch(videoFormat_)
	{
		case 0:
			switch(videoMode_)
			{
			case 0: 
				// 160x120 YUV444
				YUV444toRGB32(buffer);
				break;
			case 1:
				// 320x240 YUV222
				YUV422toRGB32(buffer);
				break;
			case 2:
				// 640x480 YUV411
				YUV411toRGB32(buffer);
				break;
			case 3:
				// 640x480 YUV422
				YUV422toRGB32(buffer);
				break;
			case 4:
				// 640x480 RGB
        RGB24toRGB32(buffer);
				//memcpy(buffer,theCamera->m_pData,640 * 480 * 3);
				break;
			case 5:
				// 640x480 MONO
				YtoRGB32(buffer);
				break;
			case 6:
				// 640x480 MONO16
				Y16toRGB32(buffer);
				break;
			default:
        retValue = false;
        qWarning("Current video mode is not supported for display and recording"); 
				break;
			} break;

		case 1:
			switch(videoMode_)
			{
			case 0:
				// 800x600 YUV422
				YUV422toRGB32(buffer);
				break;
			case 1:
				// 800x600 RGB
        //memcpy(buffer,theCamera->m_pData,800 * 600 * 3);
				break;
			case 2:
				// 800x600 MONO
				YtoRGB32(buffer);
				break;
			case 3:
				// 1024x768 YUV422
				YUV422toRGB32(buffer);
				break;
			case 4:
				// 1024x768 RGB
        //memcpy(buffer,theCamera->m_pData,1024 * 768 * 3);
				break;
			case 5:
				// 1024x768 MONO
				YtoRGB32(buffer);
				break;
			case 6:
				// 800x600 MONO16
			case 7:
				// 1024x768 MONO16
				Y16toRGB32(buffer);
				break;
			default:
        retValue = false;
        qWarning("Current video mode is not supported for display and recording"); 
				break;
			} break;

		case 2:
			switch(videoMode_)
			{
			case 0:
				// 1280x960 YUV422
				YUV422toRGB32(buffer);
				break;
			case 1:
				// 1280x960 RGB
        //memcpy(buffer,theCamera->m_pData,1280 * 960 * 3);
				break;
			case 2:
				// 1280x960 MONO
				YtoRGB32(buffer);
				break;
			case 3:
				// 1600x1200 YUV422
				YUV422toRGB32(buffer);
				break;
			case 4:
				// 1600x1200 RGB
        //memcpy(buffer,theCamera->m_pData,1600 * 1200 * 3);
				break;
			case 5:
				// 1600x1200 MONO
				YtoRGB32(buffer);
				break;
			case 6:
				// 1280x1024 MONO16
			case 7:
				// 1600x1200 MONO16
				Y16toRGB32(buffer);
				break;
			default:
        retValue = false;
        qWarning("Current video mode is not supported for display and recording"); 
				break;
			} break;

		case 7:
//			switch(theCamera->m_controlSize.m_colorCode)
//			{
//			case 0:
//				// Mono8
//				YtoRGB32(buffer);
//				break;
//			case 1:
//				// YUV 411
//				YUV411toRGB32(buffer);
//				break;
//			case 2:
//				// YUV 422
//				YUV422toRGB32(buffer);
//				break;
//			case 3:
//				// YUV 444
//				YUV444toRGB32(buffer);
//				break;
//			case 4:
//				// RGB8
//				memcpy(buffer,theCamera->m_pData,1280 * 960 * 3);
//				break;
//			case 5:
//				// Mono16
//				Y16toRGB32(buffer);
//				break;
//			case 6:
//				// RGB16
//				RGB16toRGB32(buffer);
//				break;
//			default:
//				// unsupported
//        retValue = false;
//        qWarning("Current video mode is not supported for display and recording");
//				break;
//			}
      break;

		default:
      retValue = false;
      qWarning("Current video mode is not supported for display and recording"); 
			break;
	}

  return retValue;
}

} // namespace pacpus