/***************************************************************************
 *   Copyright (C) 2006 by Miguel Armando Riem de Oliveira   *
 *   mike@BlackBandit   *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program 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 General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/


/*
 *  Copied directly from Coriander, and added conversions to bgr (backwards rgb).
 *
 */

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include "conversions.h"
#include <dc1394_control.h>

//extern void swab(const void *from, void *to, ssize_t n);

// The following #define is there for the users who experience green/purple
// images in the display. This seems to be a videocard driver problem.

#define YUYV // instead of the standard UYVY

// color conversion functions from Bart Nabbe.
// corrected by Damien: bad coeficients in YUV2RGB
#define YUV2RGB(y, u, v, r, g, b)\
  r = y + ((v*1436) >>10);\
  g = y - ((u*352 + v*731) >> 10);\
  b = y + ((u*1814) >> 10);\
  r = r < 0 ? 0 : r;\
  g = g < 0 ? 0 : g;\
  b = b < 0 ? 0 : b;\
  r = r > 255 ? 255 : r;\
  g = g > 255 ? 255 : g;\
  b = b > 255 ? 255 : b
  

#define RGB2YUV(r, g, b, y, u, v)\
  y = (306*r + 601*g + 117*b)  >> 10;\
  u = ((-172*r - 340*g + 512*b) >> 10)  + 128;\
  v = ((512*r - 429*g - 83*b) >> 10) + 128;\
  y = y < 0 ? 0 : y;\
  u = u < 0 ? 0 : u;\
  v = v < 0 ? 0 : v;\
  y = y > 255 ? 255 : y;\
  u = u > 255 ? 255 : u;\
  v = v > 255 ? 255 : v

#define CLIP(in, out)\
{\
   in = in < 0 ? 0 : in;\
   in = in > 255 ? 255 : in;\
   out=in;\
}
  
/**********************************************************************
 *
 *  CONVERSION FUNCTIONS TO UYVY 
 *
 **********************************************************************/

void
yuyv2uyvy(unsigned char *src, unsigned char *dest, int NumPixels) {
#ifdef YUYV
//  swab(src, dest, NumPixels << 1);
#else
//  memcpy(dest,src, NumPixels<<1);
#endif
}

void
uyvy2yuyv(unsigned char *src, unsigned char *dest, int NumPixels) {
#ifdef YUYV
//  swab(src, dest, NumPixels << 1);
#else
//  memcpy(dest,src, NumPixels<<1);
#endif
}
void
uyyvyy2uyvy (unsigned char *src, unsigned char *dest, int NumPixels) {
  register int i=NumPixels + (NumPixels >> 1)-1;
  register int j=(NumPixels << 1)-1;
  register int y0, y1, y2, y3, u, v;

  while (i > 0)
    {
      y3 = src[i--];
      y2 = src[i--];
      v  = src[i--];
      y1 = src[i--];
      y0 = src[i--];
      u  = src[i--];
#ifdef YUYV
      dest[j--] = v;
      dest[j--] = y3;
      dest[j--] = u;
      dest[j--] = y2;

      dest[j--] = v;
      dest[j--] = y1;
      dest[j--] = u;
      dest[j--] = y0;
#else // UYVY
      dest[j--] = y3;
      dest[j--] = v;
      dest[j--] = y2;
      dest[j--] = u;

      dest[j--] = y1;
      dest[j--] = v;
      dest[j--] = y0;
      dest[j--] = u;
#endif
    }
}

void
uyv2uyvy (unsigned char *src, unsigned char *dest, int NumPixels) {
  register int i = NumPixels + (NumPixels << 1)-1;
  register int j = (NumPixels << 1)-1;
  register int y0, y1, u0, u1, v0, v1;

  while (i > 0)
    {
      v1 = src[i--];
      y1 = src[i--];
      u1 = src[i--];
      v0 = src[i--];
      y0 = src[i--];
      u0 = src[i--];

#ifdef YUYV
      dest[j--] = (v0+v1) >> 1;
      dest[j--] = y1;
      dest[j--] = (u0+u1) >> 1;
      dest[j--] = y0;
#else // UYVY
      dest[j--] = y1;
      dest[j--] = (v0+v1) >> 1;
      dest[j--] = y0;
      dest[j--] = (u0+u1) >> 1;
#endif
    }
}


void
y2uyvy (unsigned char *src, unsigned char *dest, int NumPixels) {
  register int i= NumPixels-1;
  register int j = (NumPixels << 1)-1;
  register int y0, y1;

  while (i > 0)
    {
      y1 = src[i--];
      y0 = src[i--];
#ifdef YUYV
      dest[j--] = 128;
      dest[j--] = y1;
      dest[j--] = 128;
      dest[j--] = y0;
#else // UYVY
      dest[j--] = y1;
      dest[j--] = 128;
      dest[j--] = y0;
      dest[j--] = 128;
#endif
    }
}

void
y162uyvy (unsigned char *src, unsigned char *dest, int NumPixels, int bits) {
  register int i = (NumPixels << 1)-1;
  register int j = (NumPixels << 1)-1;
  register int y0, y1;
  //fprintf(stderr,"bpp:%d\n",bits);
  while (i > 0)
    {
      y1 = src[i--];
      y1 = (y1 + (((int)src[i--])<<8))>>(bits-8);
      y0 = src[i--];
      y0 = (y0 + (((int)src[i--])<<8))>>(bits-8);
#ifdef YUYV
      dest[j--] = 128;
      dest[j--] = y1;
      dest[j--] = 128;
      dest[j--] = y0;
#else // UYVY
      dest[j--] = y1;
      dest[j--] = 128;
      dest[j--] = y0;
      dest[j--] = 128;
#endif
    }
}

void
y162y (unsigned char *src, unsigned char *dest, int NumPixels, int bits) {
  register int i = (NumPixels<<1)-1;
  register int j = NumPixels-1;
  register int y;

  while (i > 0)
    {
      y = src[i--];
      dest[j--] = (y + (src[i--]<<8))>>(bits-8);
    }
}

void
rgb2uyvy (unsigned char *src, unsigned char *dest, int NumPixels) {
  register int i = NumPixels + ( NumPixels << 1 )-1;
  register int j = (NumPixels << 1)-1;
  register int y0, y1, u0, u1, v0, v1 ;
  register int r, g, b;

  while (i > 0)
    {
      b = (unsigned char) src[i--];
      g = (unsigned char) src[i--];
      r = (unsigned char) src[i--];
      RGB2YUV (r, g, b, y0, u0 , v0);
      b = (unsigned char) src[i--];
      g = (unsigned char) src[i--];
      r = (unsigned char) src[i--];
      RGB2YUV (r, g, b, y1, u1 , v1);
#ifdef YUYV
      dest[j--] = (v0+v1) >> 1;
      dest[j--] = y0;
      dest[j--] = (u0+u1) >> 1;
      dest[j--] = y1;
#else // UYVY
      dest[j--] = y0;
      dest[j--] = (v0+v1) >> 1;
      dest[j--] = y1;
      dest[j--] = (u0+u1) >> 1;
#endif
    }
}

void
rgb482uyvy (unsigned char *src, unsigned char *dest, int NumPixels) {
  register int i = ( (NumPixels + ( NumPixels << 1 )) << 1 ) -1;
  register int j = (NumPixels << 1)-1;
  register int y0, y1, u0, u1, v0, v1 ;
  register int r, g, b;

  while (i > 0)
    {
      i--;
      b = (unsigned char) src[i--];
      i--;
      g = (unsigned char) src[i--];
      i--;
      r = (unsigned char) src[i--];
      i--;
      RGB2YUV (r, g, b, y0, u0 , v0);
      b = (unsigned char) src[i--];
      i--;
      g = (unsigned char) src[i--];
      i--;
      r = (unsigned char) src[i--];
      RGB2YUV (r, g, b, y1, u1 , v1);

#ifdef YUYV
      dest[j--] = (v0+v1) >> 1;
      dest[j--] = y0;
      dest[j--] = (u0+u1) >> 1;
      dest[j--] = y1;
#else // UYVY
      dest[j--] = y0;
      dest[j--] = (v0+v1) >> 1;
      dest[j--] = y1;
      dest[j--] = (u0+u1) >> 1;
#endif
    }
}

/**********************************************************************
 *
 *  CONVERSION FUNCTIONS TO RGB 24bpp 
 *
 **********************************************************************/

void
rgb482rgb (unsigned char *src, unsigned char *dest, int NumPixels) {
  register int i = ((NumPixels + ( NumPixels << 1 )) << 1)-1;
  register int j = NumPixels + ( NumPixels << 1 ) -1;

  while (i > 0)
    {
      i--;
      dest[j--]=src[i--];
      i--;
      dest[j--]=src[i--];
      i--;
      dest[j--]=src[i--];
    }
}


void
uyv2rgb (unsigned char *src, unsigned char *dest, int NumPixels)
{
  register int i = (NumPixels << 1)-1;
  register int j = NumPixels + ( NumPixels << 1 ) -1;
  register int y, u, v;
  register int r, g, b;

  while (i > 0)
    {
      v = (unsigned char) src[i--] - 128;
      y = (unsigned char) src[i--];
      u = (unsigned char) src[i--] - 128;
      YUV2RGB (y, u, v, r, g, b);
      dest[j--] = b;
      dest[j--] = g;
      dest[j--] = r;
      
    }
}

void
uyv2bgr (unsigned char *src, unsigned char *dest, int NumPixels)
{
  register int i = (NumPixels << 1)-1;
  register int j = NumPixels + ( NumPixels << 1 ) -1;
  register int y, u, v;
  register int r, g, b;

  while (i > 0)
    {
      v = (unsigned char) src[i--] - 128;
      y = (unsigned char) src[i--];
      u = (unsigned char) src[i--] - 128;
      YUV2RGB (y, u, v, r, g, b);
      dest[j--] = r;
      dest[j--] = g;
      dest[j--] = b;
      
    }
}

void
uyvy2rgb (unsigned char *src, unsigned char *dest, int NumPixels) {
  register int i = (NumPixels << 1)-1;
  register int j = NumPixels + ( NumPixels << 1 ) -1;
  register int y0, y1, u, v;
  register int r, g, b;

  while (i > 0)
    {
      y1 = (unsigned char) src[i--];
      v  = (unsigned char) src[i--] - 128;
      y0 = (unsigned char) src[i--];
      u  = (unsigned char) src[i--] - 128;
      YUV2RGB (y1, u, v, r, g, b);
      dest[j--] = b;
      dest[j--] = g;
      dest[j--] = r;
      YUV2RGB (y0, u, v, r, g, b);
      dest[j--] = b;
      dest[j--] = g;
      dest[j--] = r;
    }
}


void
uyvy2bgr (unsigned char *src, unsigned char *dest, int NumPixels) {
  register int i = (NumPixels << 1)-1;
  register int j = NumPixels + ( NumPixels << 1 ) -1;
  register int y0, y1, u, v;
  register int r, g, b;

  while (i > 0)
    {
      y1 = (unsigned char) src[i--];
      v  = (unsigned char) src[i--] - 128;
      y0 = (unsigned char) src[i--];
      u  = (unsigned char) src[i--] - 128;
      YUV2RGB (y1, u, v, r, g, b);
      dest[j--] = r;
      dest[j--] = g;
      dest[j--] = b;
      YUV2RGB (y0, u, v, r, g, b);
      dest[j--] = r;
      dest[j--] = g;
      dest[j--] = b;
    }
}


void
uyyvyy2rgb (unsigned char *src, unsigned char *dest, int NumPixels) {
  register int i = NumPixels + ( NumPixels >> 1 )-1;
  register int j = NumPixels + ( NumPixels << 1 )-1;
  register int y0, y1, y2, y3, u, v;
  register int r, g, b;
  
  while (i > 0)
    {
      y3 = (unsigned char) src[i--];
      y2 = (unsigned char) src[i--];
      v  = (unsigned char) src[i--] - 128;
      y1 = (unsigned char) src[i--];
      y0 = (unsigned char) src[i--];
      u  = (unsigned char) src[i--] - 128;
      YUV2RGB (y3, u, v, r, g, b);
      dest[j--] = b;
      dest[j--] = g;
      dest[j--] = r;
      YUV2RGB (y2, u, v, r, g, b);
      dest[j--] = b;
      dest[j--] = g;
      dest[j--] = r;
      YUV2RGB (y1, u, v, r, g, b);
      dest[j--] = b;
      dest[j--] = g;
      dest[j--] = r;
      YUV2RGB (y0, u, v, r, g, b);
      dest[j--] = b;
      dest[j--] = g;
      dest[j--] = r;
    }
}

void
uyyvyy2bgr (unsigned char *src, unsigned char *dest, int NumPixels) {
  register int i = NumPixels + ( NumPixels >> 1 )-1;
  register int j = NumPixels + ( NumPixels << 1 )-1;
  register int y0, y1, y2, y3, u, v;
  register int r, g, b;
  
  while (i > 0)
    {
      y3 = (unsigned char) src[i--];
      y2 = (unsigned char) src[i--];
      v  = (unsigned char) src[i--] - 128;
      y1 = (unsigned char) src[i--];
      y0 = (unsigned char) src[i--];
      u  = (unsigned char) src[i--] - 128;
      YUV2RGB (y3, u, v, r, g, b);
      dest[j--] = r;
      dest[j--] = g;
      dest[j--] = b;
      YUV2RGB (y2, u, v, r, g, b);
      dest[j--] = r;
      dest[j--] = g;
      dest[j--] = b;
      YUV2RGB (y1, u, v, r, g, b);
      dest[j--] = r;
      dest[j--] = g;
      dest[j--] = b;
      YUV2RGB (y0, u, v, r, g, b);
      dest[j--] = r;
      dest[j--] = g;
      dest[j--] = b;
    }
}

void
y2rgb (unsigned char *src, unsigned char *dest, int NumPixels) {
  register int i = NumPixels-1;
  register int j = NumPixels + ( NumPixels << 1 )-1;
  register int y;

  while (i > 0)
    {
      y = (unsigned char) src[i--];
      dest[j--] = y;
      dest[j--] = y;
      dest[j--] = y;
    }
}

void
y162rgb (unsigned char *src, unsigned char *dest, int NumPixels, int bits) {
  register int i = (NumPixels << 1)-1;
  register int j = NumPixels + ( NumPixels << 1 )-1;
  register int y;

  while (i > 0)
    {
      
      y = src[i--];
      y = (y + (src[i--]<<8))>>(bits-8);
      dest[j--] = y;
      dest[j--] = y;
      dest[j--] = y;
    }
}

/****************************************************************
 *     Color conversion functions for cameras that can          * 
 * output raw-Bayer pattern images, such as some Basler cameras *
 * Most of the algos presented here com from                    *
 * http://ise0.Stanford.EDU/~tingchen/main.htm                  *
 * and have been converted from Matlab to C                     *
 ****************************************************************/

void
BayerNearestNeighbor(unsigned char *src, unsigned char *dest, int sx, int sy, bayer_pattern_t type)
{
  unsigned char *outR, *outG, *outB;
  register int i,j;

  // sx and sy should be even
  switch (type)
    {
    case BAYER_PATTERN_GRBG:
    case BAYER_PATTERN_BGGR:
      outR=&dest[0];
      outG=&dest[1];
      outB=&dest[2];
      break;
    case BAYER_PATTERN_GBRG:
    case BAYER_PATTERN_RGGB:
      outR=&dest[2];
      outG=&dest[1];
      outB=&dest[0];
      break;
    default:
      outR=NULL;outG=NULL;outB=NULL;
      break;
    }

  switch (type)
    {
    case BAYER_PATTERN_GRBG: //-------------------------------------------
    case BAYER_PATTERN_GBRG:
      // copy original RGB data to output images
      for (i=0;i<sy;i+=2)
	for (j=0;j<sx;j+=2)
	  {
	    outG[(i*sx+j)*3]=src[i*sx+j];
	    outG[((i+1)*sx+(j+1))*3]=src[(i+1)*sx+(j+1)];
	    outR[(i*sx+j+1)*3]=src[i*sx+j+1];
	    outB[((i+1)*sx+j)*3]=src[(i+1)*sx+j];
	  }
      
      // R channel
      for (i=0;i<sy;i+=2)
	for (j=0;j<sx-1;j+=2)
	  {
	    outR[(i*sx+j)*3]=outR[(i*sx+j+1)*3];
	    outR[((i+1)*sx+j+1)*3]=outR[(i*sx+j+1)*3];
	    outR[((i+1)*sx+j)*3]=outR[(i*sx+j+1)*3];
	  }
      
      // B channel
      for (i=0;i<sy-1;i+=2)//every two lines
	for (j=0;j<sx-1;j+=2)
	  {
	    outB[(i*sx+j)*3]=outB[((i+1)*sx+j)*3];
	    outB[(i*sx+j+1)*3]=outB[((i+1)*sx+j)*3];
	    outB[((i+1)*sx+j+1)*3]=outB[((i+1)*sx+j)*3];
	  }
      
      // using lower direction for G channel
      
      // G channel
      for (i=0;i<sy-1;i+=2)//every two lines
	for (j=1;j<sx;j+=2)
	  outG[(i*sx+j)*3]=outG[((i+1)*sx+j)*3];
      
      for (i=1;i<sy-2;i+=2)//every two lines
	for (j=0;j<sx-1;j+=2)
	  outG[(i*sx+j)*3]=outG[((i+1)*sx+j)*3];
      
      // copy it for the next line
      for (j=0;j<sx-1;j+=2)
	outG[((sy-1)*sx+j)*3]=outG[((sy-2)*sx+j)*3];
      
      break;
    case BAYER_PATTERN_BGGR: //-------------------------------------------
    case BAYER_PATTERN_RGGB:
      // copy original data
      for (i=0;i<sy;i+=2)
	for (j=0;j<sx;j+=2)
	  {
	    outB[(i*sx+j)*3]=src[i*sx+j];
	    outR[((i+1)*sx+(j+1))*3]=src[(i+1)*sx+(j+1)];
	    outG[(i*sx+j+1)*3]=src[i*sx+j+1];
	    outG[((i+1)*sx+j)*3]=src[(i+1)*sx+j];
	  }

      // R channel
      for (i=0;i<sy;i+=2)
	for (j=0;j<sx-1;j+=2)
	  {
	    outR[(i*sx+j)*3]=outR[((i+1)*sx+j+1)*3];
	    outR[(i*sx+j+1)*3]=outR[((i+1)*sx+j+1)*3];
	    outR[((i+1)*sx+j)*3]=outR[((i+1)*sx+j+1)*3];
	  }
      
      // B channel
      for (i=0;i<sy-1;i+=2)//every two lines
	for (j=0;j<sx-1;j+=2)
	  {
	    outB[((i+1)*sx+j)*3]=outB[(i*sx+j)*3];
	    outB[(i*sx+j+1)*3]=outB[(i*sx+j)*3];
	    outB[((i+1)*sx+j+1)*3]=outB[(i*sx+j)*3];
	  }

      // using lower direction for G channel
      
      // G channel
      for (i=0;i<sy-1;i+=2)//every two lines
	for (j=0;j<sx-1;j+=2)
	  outG[(i*sx+j)*3]=outG[((i+1)*sx+j)*3];
      
      for (i=1;i<sy-2;i+=2)//every two lines
	for (j=0;j<sx-1;j+=2)
	  outG[(i*sx+j+1)*3]=outG[((i+1)*sx+j+1)*3];
      
      // copy it for the next line
      for (j=0;j<sx-1;j+=2)
	outG[((sy-1)*sx+j+1)*3]=outG[((sy-2)*sx+j+1)*3];

      break;

    default:  //-------------------------------------------
      fprintf(stderr,"Bad Bayer pattern ID\n");
      break;
    }
}


void
BayerEdgeSense(unsigned char *src, unsigned char *dest, int sx, int sy, bayer_pattern_t type)
{
  unsigned char *outR, *outG, *outB;
  register int i,j;
  int dh, dv;
  int tmp;

  // sx and sy should be even
  switch (type)
    {
    case BAYER_PATTERN_GRBG:
    case BAYER_PATTERN_BGGR:
      outR=&dest[0];
      outG=&dest[1];
      outB=&dest[2];
      break;
    case BAYER_PATTERN_GBRG:
    case BAYER_PATTERN_RGGB:
      outR=&dest[2];
      outG=&dest[1];
      outB=&dest[0];
      break;
    default:
      outR=NULL;outG=NULL;outB=NULL;
      break;
    }

  switch (type)
    {
    case BAYER_PATTERN_GRBG://---------------------------------------------------------
    case BAYER_PATTERN_GBRG:
      // copy original RGB data to output images
      for (i=0;i<sy;i+=2)
	for (j=0;j<sx;j+=2)
	  {
	    outG[(i*sx+j)*3]=src[i*sx+j];
	    outG[((i+1)*sx+(j+1))*3]=src[(i+1)*sx+(j+1)];
	    outR[(i*sx+j+1)*3]=src[i*sx+j+1];
	    outB[((i+1)*sx+j)*3]=src[(i+1)*sx+j];
	  }
      
      // process GREEN channel
      for (i=3;i<sy-2;i+=2)
	for (j=2;j<sx-3;j+=2)
	  {
	    dh=abs((outB[(i*sx+j-2)*3]+outB[(i*sx+j+2)*3])/2-outB[(i*sx+j)*3]);
	    dv=abs((outB[((i-2)*sx+j)*3]+outB[((i+2)*sx+j)*3])/2-outB[(i*sx+j)*3]);
	    if (dh<dv)
	      tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3])/2;
	    else
	      if (dh>dv)
		tmp=(outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/2;
	      else
		tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3]+outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/4;
	    CLIP(tmp,outG[(i*sx+j)*3]);
	  }
      
      for (i=2;i<sy-3;i+=2)
	for (j=3;j<sx-2;j+=2)
	  {
	    dh=abs((outR[(i*sx+j-2)*3]+outR[(i*sx+j+2)*3])/2-outR[(i*sx+j)*3]);
	    dv=abs((outR[((i-2)*sx+j)*3]+outR[((i+2)*sx+j)*3])/2-outR[(i*sx+j)*3]);
	    if (dh<dv)
	      tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3])/2;
	    else
	      if (dh>dv)
		tmp=(outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/2;
	      else
		tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3]+outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/4;
	    CLIP(tmp,outG[(i*sx+j)*3]);
	  }
      
      // process RED channel
      for (i=0;i<sy-1;i+=2)
	for (j=2;j<sx-1;j+=2)
	  {
	    tmp=outG[(i*sx+j)*3]+(outR[(i*sx+j-1)*3]-outG[(i*sx+j-1)*3]+
				  outR[(i*sx+j+1)*3]-outG[(i*sx+j+1)*3])/2;
	    CLIP(tmp,outR[(i*sx+j)*3]);
	  }
      for (i=1;i<sy-2;i+=2)
	{
	  for (j=1;j<sx;j+=2)
	    {
	      tmp=outG[(i*sx+j)*3]+(outR[((i-1)*sx+j)*3]-outG[((i-1)*sx+j)*3]+
				    outR[((i+1)*sx+j)*3]-outG[((i+1)*sx+j)*3])/2;
	      CLIP(tmp,outR[(i*sx+j)*3]);
	    }
	  for (j=2;j<sx-1;j+=2)
	    {
	      tmp=outG[(i*sx+j)*3]+(outR[((i-1)*sx+j-1)*3]-outG[((i-1)*sx+j-1)*3]+
				    outR[((i-1)*sx+j+1)*3]-outG[((i-1)*sx+j+1)*3]+
				    outR[((i+1)*sx+j-1)*3]-outG[((i+1)*sx+j-1)*3]+
				    outR[((i+1)*sx+j+1)*3]-outG[((i+1)*sx+j+1)*3])/4;
	      CLIP(tmp,outR[(i*sx+j)*3]);
	    }
	}
      
      // process BLUE channel
      for (i=1;i<sy;i+=2)
	for (j=1;j<sx-2;j+=2)
	  {
	    tmp=outG[(i*sx+j)*3]+(outB[(i*sx+j-1)*3]-outG[(i*sx+j-1)*3]+
				  outB[(i*sx+j+1)*3]-outG[(i*sx+j+1)*3])/2;
	    CLIP(tmp,outB[(i*sx+j)*3]);
	  }
      
      for (i=2;i<sy-1;i+=2)
	{
	  for (j=0;j<sx-1;j+=2)
	    {
	      tmp=outG[(i*sx+j)*3]+(outB[((i-1)*sx+j)*3]-outG[((i-1)*sx+j)*3]+
				    outB[((i+1)*sx+j)*3]-outG[((i+1)*sx+j)*3])/2;
	      CLIP(tmp,outB[(i*sx+j)*3]);
	    }
	  for (j=1;j<sx-2;j+=2)
	    {
	      tmp=outG[(i*sx+j)*3]+(outB[((i-1)*sx+j-1)*3]-outG[((i-1)*sx+j-1)*3]+
				    outB[((i-1)*sx+j+1)*3]-outG[((i-1)*sx+j+1)*3]+
				    outB[((i+1)*sx+j-1)*3]-outG[((i+1)*sx+j-1)*3]+
				    outB[((i+1)*sx+j+1)*3]-outG[((i+1)*sx+j+1)*3])/4;
	      CLIP(tmp,outB[(i*sx+j)*3]);
	    }
	}
      break;

    case BAYER_PATTERN_BGGR: //---------------------------------------------------------
    case BAYER_PATTERN_RGGB:
      // copy original RGB data to output images
      for (i=0;i<sy;i+=2)
	for (j=0;j<sx;j+=2)
	  {
	    outB[(i*sx+j)*3]=src[i*sx+j];
	    outR[((i+1)*sx+(j+1))*3]=src[(i+1)*sx+(j+1)];
	    outG[(i*sx+j+1)*3]=src[i*sx+j+1];
	    outG[((i+1)*sx+j)*3]=src[(i+1)*sx+j];
	  }
      
      // process GREEN channel
      for (i=2;i<sy-2;i+=2)
	for (j=2;j<sx-3;j+=2)
	  {
	    dh=abs((outB[(i*sx+j-2)*3]+outB[(i*sx+j+2)*3])/2-outB[(i*sx+j)*3]);
	    dv=abs((outB[((i-2)*sx+j)*3]+outB[((i+2)*sx+j)*3])/2-outB[(i*sx+j)*3]);
	    if (dh<dv)
	      tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3])/2;
	    else
	      if (dh>dv)
		tmp=(outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/2;
	      else
		tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3]+outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/4;
	    CLIP(tmp,outG[(i*sx+j)*3]);
	  }
      
      for (i=3;i<sy-3;i+=2)
	for (j=3;j<sx-2;j+=2)
	  {
	    dh=abs((outR[(i*sx+j-2)*3]+outR[(i*sx+j+2)*3])/2-outR[(i*sx+j)*3]);
	    dv=abs((outR[((i-2)*sx+j)*3]+outR[((i+2)*sx+j)*3])/2-outR[(i*sx+j)*3]);
	    if (dh<dv)
	      tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3])/2;
	    else
	      if (dh>dv)
		tmp=(outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/2;
	      else
		tmp=(outG[(i*sx+j-1)*3]+outG[(i*sx+j+1)*3]+outG[((i-1)*sx+j)*3]+outG[((i+1)*sx+j)*3])/4;
	    CLIP(tmp,outG[(i*sx+j)*3]);
	  }
      
      // process RED channel
      for (i=1;i<sy-1;i+=2) // G-points (1/2)
	for (j=2;j<sx-1;j+=2)
	  {
	    tmp=outG[(i*sx+j)*3]+(outR[(i*sx+j-1)*3]-outG[(i*sx+j-1)*3]+
				  outR[(i*sx+j+1)*3]-outG[(i*sx+j+1)*3])/2;
	    CLIP(tmp,outR[(i*sx+j)*3]);
	  }
      for (i=2;i<sy-2;i+=2) 
	{
	  for (j=1;j<sx;j+=2) // G-points (2/2)
	    {
	      tmp=outG[(i*sx+j)*3]+(outR[((i-1)*sx+j)*3]-outG[((i-1)*sx+j)*3]+
				    outR[((i+1)*sx+j)*3]-outG[((i+1)*sx+j)*3])/2;
	      CLIP(tmp,outR[(i*sx+j)*3]);
	    }
	  for (j=2;j<sx-1;j+=2) // B-points
	    {
	      tmp=outG[(i*sx+j)*3]+(outR[((i-1)*sx+j-1)*3]-outG[((i-1)*sx+j-1)*3]+
				    outR[((i-1)*sx+j+1)*3]-outG[((i-1)*sx+j+1)*3]+
				    outR[((i+1)*sx+j-1)*3]-outG[((i+1)*sx+j-1)*3]+
				    outR[((i+1)*sx+j+1)*3]-outG[((i+1)*sx+j+1)*3])/4;
	      CLIP(tmp,outR[(i*sx+j)*3]);
	    }
	}
      
      // process BLUE channel
      for (i=0;i<sy;i+=2)
	for (j=1;j<sx-2;j+=2)
	  {
	    tmp=outG[(i*sx+j)*3]+(outB[(i*sx+j-1)*3]-outG[(i*sx+j-1)*3]+
				  outB[(i*sx+j+1)*3]-outG[(i*sx+j+1)*3])/2;
	    CLIP(tmp,outB[(i*sx+j)*3]);
	  }
      
      for (i=1;i<sy-1;i+=2)
	{
	  for (j=0;j<sx-1;j+=2)
	    {
	      tmp=outG[(i*sx+j)*3]+(outB[((i-1)*sx+j)*3]-outG[((i-1)*sx+j)*3]+
				    outB[((i+1)*sx+j)*3]-outG[((i+1)*sx+j)*3])/2;
	      CLIP(tmp,outB[(i*sx+j)*3]);
	    }
	  for (j=1;j<sx-2;j+=2)
	    {
	      tmp=outG[(i*sx+j)*3]+(outB[((i-1)*sx+j-1)*3]-outG[((i-1)*sx+j-1)*3]+
				    outB[((i-1)*sx+j+1)*3]-outG[((i-1)*sx+j+1)*3]+
				    outB[((i+1)*sx+j-1)*3]-outG[((i+1)*sx+j-1)*3]+
				    outB[((i+1)*sx+j+1)*3]-outG[((i+1)*sx+j+1)*3])/4;
	      CLIP(tmp,outB[(i*sx+j)*3]);
	    }
	}
      break;
    default: //---------------------------------------------------------
      fprintf(stderr,"Bad bayer pattern ID\n");
      break;
    }
}

void
BayerDownsample(unsigned char *src, unsigned char *dest, int sx, int sy, bayer_pattern_t type)
{
  unsigned char *outR, *outG, *outB;
  register int i,j;
  int tmp;

  // sx and sy should be even

  switch (type)
    {
    case BAYER_PATTERN_GRBG:
    case BAYER_PATTERN_BGGR:
      outR=&dest[0];
      outG=&dest[1];
      outB=&dest[2];
      break;
    case BAYER_PATTERN_GBRG:
    case BAYER_PATTERN_RGGB:
      outR=&dest[2];
      outG=&dest[1];
      outB=&dest[0];
      break;
    default:
      outR=NULL;outG=NULL;outB=NULL;
      break;
    }

  switch (type)
    {
    case BAYER_PATTERN_GRBG://---------------------------------------------------------
    case BAYER_PATTERN_GBRG:
      for (i=0;i<sy;i+=2)
	for (j=0;j<sx;j+=2)
	  {
	    tmp=((src[i*sx+j]+src[(i+1)*sx+(j+1)])>>1);
	    CLIP(tmp,outG[(((i*sx)>>2)+(j>>1))*3]);
	    tmp=src[i*sx+j+1];
	    CLIP(tmp,outR[(((i*sx)>>2)+(j>>1))*3]);
	    tmp=src[(i+1)*sx+j];
	    CLIP(tmp,outB[(((i*sx)>>2)+(j>>1))*3]);
	  }
      break;
    case BAYER_PATTERN_BGGR://---------------------------------------------------------
    case BAYER_PATTERN_RGGB:
      for (i=0;i<sy;i+=2)
	for (j=0;j<sx;j+=2)
	  {
	    tmp=((src[(i+1)*sx+j]+src[i*sx+(j+1)])>>1);
	    CLIP(tmp,outG[(((i*sx)>>2)+(j>>1))*3]);
	    tmp=src[(i+1)*sx+j+1];
	    CLIP(tmp,outR[(((i*sx)>>2)+(j>>1))*3]);
	    tmp=src[i*sx+j];
	    CLIP(tmp,outB[(((i*sx)>>2)+(j>>1))*3]);
	  }
      break;
    default: //---------------------------------------------------------
      fprintf(stderr,"Bad bayer pattern ID\n");
      break;
    }

}

// change a 16bit stereo image (8bit/channel) into two 8bit images on top
// of each other
void
StereoDecode (unsigned char *src, unsigned char *dest, int NumPixels)
{
  register int i = (NumPixels<<1)-1;
  register int j = NumPixels-1;
  register int k = (NumPixels<<1)-1;

  while (i > 0)
    {
      dest[k--] = src[i--];
      dest[j--] = src[i--];
    }
}

void
convert_to_rgb(unsigned char *src, unsigned char *dest, int mode, int width, int height, int f7_colormode, int bayer, int bits)
{
  if (bayer==NO_BAYER_DECODING)
    {
      switch(mode)
	{
	case MODE_160x120_YUV444:
	  uyv2rgb(src,dest,width*height);
	  break;
	case MODE_320x240_YUV422:
	case MODE_640x480_YUV422:
	case MODE_800x600_YUV422:
	case MODE_1024x768_YUV422:
	case MODE_1280x960_YUV422:
	case MODE_1600x1200_YUV422:
	  uyvy2rgb(src,dest,width*height);
	  break;
	case MODE_640x480_YUV411:
	  uyyvyy2rgb(src,dest,width*height);
	  break;
	case MODE_640x480_RGB:
	case MODE_800x600_RGB:
	case MODE_1024x768_RGB:
	case MODE_1280x960_RGB:
	case MODE_1600x1200_RGB:
	  memcpy(dest,src,3*width*height);
	  break;
	case MODE_640x480_MONO:
	case MODE_800x600_MONO:
	case MODE_1024x768_MONO:
	case MODE_1280x960_MONO:
	case MODE_1600x1200_MONO:
	  y2rgb(src,dest,width*height);
	  break;
	case MODE_640x480_MONO16:
	case MODE_800x600_MONO16:
	case MODE_1024x768_MONO16:
	case MODE_1280x960_MONO16:
	case MODE_1600x1200_MONO16:
	  y162rgb(src,dest,width*height,bits);
	  break;
	case MODE_FORMAT7_0:
	case MODE_FORMAT7_1:
	case MODE_FORMAT7_2:
	case MODE_FORMAT7_3:
	case MODE_FORMAT7_4:
	case MODE_FORMAT7_5:
	case MODE_FORMAT7_6:
	case MODE_FORMAT7_7:
	  switch (f7_colormode)
	    {
	    case COLOR_FORMAT7_MONO8:
	      y2rgb(src,dest,width*height);
	      break;
	    case COLOR_FORMAT7_YUV411:
	      uyyvyy2rgb(src,dest,width*height);
	      break;
	    case COLOR_FORMAT7_YUV422:
	      uyvy2rgb(src,dest,width*height);
	      break;
	    case COLOR_FORMAT7_YUV444:
	      uyv2rgb(src,dest,width*height);
	      break;
	    case COLOR_FORMAT7_RGB8:
	      memcpy(dest,src,3*width*height);
	      break;
	    case COLOR_FORMAT7_MONO16:
	      y162rgb(src,dest,width*height,bits);
	      break;
	    case COLOR_FORMAT7_RGB16:
	      rgb482rgb(src,dest,width*height);
	      break;
	    }
	  break;
	}
    }
  else // we force RGB mode, thus use memcpy
    memcpy(dest,src,3*width*height);
    
}


void
convert_to_bgr(unsigned char *src, unsigned char *dest, int mode, int width, int height, int f7_colormode, int bayer, int bits)
{
    if (bayer==NO_BAYER_DECODING)
    {
        switch(mode)
        {
        case MODE_160x120_YUV444:
            uyv2bgr(src,dest,width*height);
            break;
        case MODE_320x240_YUV422:
        case MODE_640x480_YUV422:
        case MODE_800x600_YUV422:
        case MODE_1024x768_YUV422:
        case MODE_1280x960_YUV422:
        case MODE_1600x1200_YUV422:
            uyvy2bgr(src,dest,width*height);
            break;
        case MODE_640x480_YUV411:
            uyyvyy2bgr(src,dest,width*height);
            break;
        default:
            fprintf(stderr,"Can't do this conversion\n");
            exit(1);
            break;
        }
    }
    else // we force RGB mode, thus use memcpy
        memcpy(dest,src,3*width*height);
}