/*****************************************************************************
 * Format-Conversion for gyachi
 ****************************************************************************/

/*
 * BAYER2RGB24 ROUTINE TAKEN FROM:
 *
 * Sonix SN9C101 based webcam basic I/F routines
 * Copyright (C) 2004 Takafumi Mizuno <taka-qce@ls-a.jp>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */

#include "v4l-fmtconv.h"
#include <stdint.h>

void
bayer2rgb24 (unsigned char *dst, unsigned char *src, long int WIDTH,
	     long int HEIGHT)
{
  long int i;
  unsigned char *rawpt, *scanpt;
  long int size;

  rawpt = src;
  scanpt = dst;
  size = WIDTH * HEIGHT;

  for (i = 0; i < size; i++)
    {
      if ((i / WIDTH) % 2 == 0)
	{
	  if ((i % 2) == 0)
	    {
	      /* B */
	      if ((i > WIDTH) && ((i % WIDTH) > 0))
		{
		  *scanpt++ = (*(rawpt - WIDTH - 1) + *(rawpt - WIDTH + 1) + *(rawpt + WIDTH - 1) + *(rawpt + WIDTH + 1)) / 4;	/* R */
		  *scanpt++ = (*(rawpt - 1) + *(rawpt + 1) + *(rawpt + WIDTH) + *(rawpt - WIDTH)) / 4;	/* G */
		  *scanpt++ = *rawpt;	/* B */
		}
	      else
		{
		  /* first line or left column */
		  *scanpt++ = *(rawpt + WIDTH + 1);	/* R */
		  *scanpt++ = (*(rawpt + 1) + *(rawpt + WIDTH)) / 2;	/* G */
		  *scanpt++ = *rawpt;	/* B */
		}
	    }
	  else
	    {
	      /* (B)G */
	      if ((i > WIDTH) && ((i % WIDTH) < (WIDTH - 1)))
		{
		  *scanpt++ = (*(rawpt + WIDTH) + *(rawpt - WIDTH)) / 2;	/* R */
		  *scanpt++ = *rawpt;	/* G */
		  *scanpt++ = (*(rawpt - 1) + *(rawpt + 1)) / 2;	/* B */
		}
	      else
		{
		  /* first line or right column */
		  *scanpt++ = *(rawpt + WIDTH);	/* R */
		  *scanpt++ = *rawpt;	/* G */
		  *scanpt++ = *(rawpt - 1);	/* B */
		}
	    }
	}
      else
	{
	  if ((i % 2) == 0)
	    {
	      /* G(R) */
	      if ((i < (WIDTH * (HEIGHT - 1))) && ((i % WIDTH) > 0))
		{
		  *scanpt++ = (*(rawpt - 1) + *(rawpt + 1)) / 2;	/* R */
		  *scanpt++ = *rawpt;	/* G */
		  *scanpt++ = (*(rawpt + WIDTH) + *(rawpt - WIDTH)) / 2;	/* B */
		}
	      else
		{
		  /* bottom line or left column */
		  *scanpt++ = *(rawpt + 1);	/* R */
		  *scanpt++ = *rawpt;	/* G */
		  *scanpt++ = *(rawpt - WIDTH);	/* B */
		}
	    }
	  else
	    {
	      /* R */
	      if (i < (WIDTH * (HEIGHT - 1)) && ((i % WIDTH) < (WIDTH - 1)))
		{
		  *scanpt++ = *rawpt;	/* R */
		  *scanpt++ = (*(rawpt - 1) + *(rawpt + 1) + *(rawpt - WIDTH) + *(rawpt + WIDTH)) / 4;	/* G */
		  *scanpt++ = (*(rawpt - WIDTH - 1) + *(rawpt - WIDTH + 1) + *(rawpt + WIDTH - 1) + *(rawpt + WIDTH + 1)) / 4;	/* B */
		}
	      else
		{
		  /* bottom line or right column */
		  *scanpt++ = *rawpt;	/* R */
		  *scanpt++ = (*(rawpt - 1) + *(rawpt - WIDTH)) / 2;	/* G */
		  *scanpt++ = *(rawpt - WIDTH - 1);	/* B */
		}
	    }
	}
      rawpt++;
    }

}

/* The following conversion routines were taken from drivers/usb/ov511.c
 * in the Mandrake Linux 2.4.21 kernel.
 */

static int force_rgb = 1;

/* LIMIT: convert a 16.16 fixed-point value to a byte, with clipping. */
#define LIMIT(x) ((x)>0xffffff?0xff: ((x)<=0xffff?0:((x)>>16)))

static inline void
move_420_block(int yTL, int yTR, int yBL, int yBR, int u, int v,
	       int rowPixels, unsigned char * rgb, int bits)
{
	const int rvScale = 91881;
	const int guScale = -22553;
	const int gvScale = -46801;
	const int buScale = 116129;
	const int yScale  = 65536;
	int r, g, b;

	g = guScale * u + gvScale * v;
	if (force_rgb) {
		r = buScale * u;
		b = rvScale * v;
	} else {
		r = rvScale * v;
		b = buScale * u;
	}

	yTL *= yScale; yTR *= yScale;
	yBL *= yScale; yBR *= yScale;

	if (bits == 24) {
		/* Write out top two pixels */
		rgb[0] = LIMIT(b+yTL); rgb[1] = LIMIT(g+yTL);
		rgb[2] = LIMIT(r+yTL);

		rgb[3] = LIMIT(b+yTR); rgb[4] = LIMIT(g+yTR);
		rgb[5] = LIMIT(r+yTR);

		/* Skip down to next line to write out bottom two pixels */
		rgb += 3 * rowPixels;
		rgb[0] = LIMIT(b+yBL); rgb[1] = LIMIT(g+yBL);
		rgb[2] = LIMIT(r+yBL);

		rgb[3] = LIMIT(b+yBR); rgb[4] = LIMIT(g+yBR);
		rgb[5] = LIMIT(r+yBR);
	} else if (bits == 16) {
		/* Write out top two pixels */
		rgb[0] = ((LIMIT(b+yTL) >> 3) & 0x1F)
			| ((LIMIT(g+yTL) << 3) & 0xE0);
		rgb[1] = ((LIMIT(g+yTL) >> 5) & 0x07)
			| (LIMIT(r+yTL) & 0xF8);

		rgb[2] = ((LIMIT(b+yTR) >> 3) & 0x1F)
			| ((LIMIT(g+yTR) << 3) & 0xE0);
		rgb[3] = ((LIMIT(g+yTR) >> 5) & 0x07)
			| (LIMIT(r+yTR) & 0xF8);

		/* Skip down to next line to write out bottom two pixels */
		rgb += 2 * rowPixels;

		rgb[0] = ((LIMIT(b+yBL) >> 3) & 0x1F)
			| ((LIMIT(g+yBL) << 3) & 0xE0);
		rgb[1] = ((LIMIT(g+yBL) >> 5) & 0x07)
			| (LIMIT(r+yBL) & 0xF8);

		rgb[2] = ((LIMIT(b+yBR) >> 3) & 0x1F)
			| ((LIMIT(g+yBR) << 3) & 0xE0);
		rgb[3] = ((LIMIT(g+yBR) >> 5) & 0x07)
			| (LIMIT(r+yBR) & 0xF8);
	}
}


/* Converts from planar YUV420 to RGB24. */
void
yuv420p_to_rgb(unsigned char *pIn0, unsigned char *pOut0, int bits, int width, int height)
{
	const int numpix = width * height;
	const int bytes = bits >> 3;
	int i, j, y00, y01, y10, y11, u, v;
	unsigned char *pY = pIn0;
	unsigned char *pU = pY + numpix;
	unsigned char *pV = pU + numpix / 4;
	unsigned char *pOut = pOut0;

	for (j = 0; j <= height - 2; j += 2) {
		for (i = 0; i <= width - 2; i += 2) {
			y00 = *pY;
			y01 = *(pY + 1);
			y10 = *(pY + width);
			y11 = *(pY + width + 1);
			u = (*pU++) - 128;
			v = (*pV++) - 128;

			move_420_block(y00, y01, y10, y11, u, v,
				       width, pOut, bits);

			pY += 2;
			pOut += 2 * bytes;
		}
		pY += width;
		pOut += width * bytes;
	}
}


/* The following function is based on code by Vincent Hourdin.
 * http://vinvin.dyndns.org/projects/
 *
 * Faster integer maths from camE by Tom Gilbert.
 * http://linuxbrit.co.uk/camE/
*/
void
yuv420p_to_rgb24(uint32_t width, uint32_t height, unsigned char *dst, void *src)
{
	uint8_t *yptr, *uptr, *vptr;
	uint32_t x, y, p, o;
	
	/* Setup separate pointers to Y, U and V buffers. */
	yptr = (uint8_t *) src;
	uptr = yptr + (width * height);
	vptr = uptr + (width * height / 4);
	o = 0;
	p = 0;
	
	for(y = 0; y < height; y++)
	{
		for(x = 0; x < width; x++)
		{
			int r, g, b;
			int y, u, v;
			
			y = *(yptr++) << 8;
			u = uptr[p] - 128;
			v = vptr[p] - 128;
			
			r = (y + (359 * v)) >> 8;
			g = (y - (88 * u) - (183 * v)) >> 8;
			b = (y + (454 * u)) >> 8;
			
			*(dst++) = (b > 255) ? 255 : ((b < 0) ? 0 : b);
			*(dst++) = (g > 255) ? 255 : ((g < 0) ? 0 : g);
			*(dst++) = (r > 255) ? 255 : ((r < 0) ? 0 : r);
			
			if(x & 1) p++;
		}
		
		if(!(y & 1)) p -= width / 2;
	}
}

/*
void
jpeg_to_rgb24(uint32_t width, uint32_t height, unsigned char *dst, void *src, size_t framelen)
{
	uint32_t x, y;
	gdImage *im;
	
	im = gdImageCreateFromJpegPtr(framelen, src);
	if(!im) return(-1);
	
	for(y = 0; y < height; y++)
		for(x = 0; x < width; x++)
		{
			int c = gdImageGetPixel(im, x, y);
			
			*(dst++) = (c & 0x0000FF);
			*(dst++) = (c & 0x00FF00) >> 8;
			*(dst++) = (c & 0xFF0000) >> 16;
		}
	
	gdImageDestroy(im);
}
*/

/* Conversion from yuv422p to rgb24 */
void
yuv422p_to_rgb24(uint32_t width, uint32_t height, unsigned char *dst, void *src)
{
	uint8_t *yptr, *uptr, *vptr;
	uint32_t x, y, p;
	
	/* Setup separate pointers to Y, U and V buffers. */
	yptr = (uint8_t *) src;
	uptr = yptr + (width * height);
	vptr = uptr + (width * height / 2);
	p = 0;
	
	for(y = 0; y < height; y++)
	{
		for(x = 0; x < width; x++)
		{
			int r, g, b;
			int y, u, v;
			
			y = *(yptr++) << 8;
			u = uptr[p] - 128;
			v = vptr[p] - 128;
			
			r = (y + (359 * v)) >> 8;
			g = (y - (88 * u) - (183 * v)) >> 8;
			b = (y + (454 * u)) >> 8;
			
			*(dst++) = (b > 255) ? 255 : ((b < 0) ? 0 : b);
			*(dst++) = (g > 255) ? 255 : ((g < 0) ? 0 : g);
			*(dst++) = (r > 255) ? 255 : ((r < 0) ? 0 : r);
			
			if(x & 1) p++;
		}
	}
}


void
rgb32_to_rgb24(uint32_t width, uint32_t height, unsigned char *dst, void *src)
{
	uint8_t *bitmap = (uint8_t *) src;
	uint32_t i = width * height;
	
	for(; i > 0; i--)
	{
		*(dst++) = *(bitmap++);
		*(dst++) = *(bitmap++);
		*(dst++) = *(bitmap++);
		bitmap++;
	}
}

void
rgb24_to_rgb24(uint32_t width, uint32_t height, unsigned char *dst, void *src)
{
	uint8_t *bitmap = (uint8_t *) src;
	uint32_t i = width * height;
	
	for(; i > 0; i--) 
	 {
	  *(dst++) = *(bitmap+2);
	  *(dst++) = *(bitmap+1);
	  *(dst++) = *(bitmap);
	  bitmap+=3;
	 }
}

void
rgb565_to_rgb24(uint32_t width, uint32_t height,
                          unsigned char *dst, void *src)
{
	uint16_t *bitmap = (uint16_t *) src;
	uint32_t i = width * height;
	
	for(; i > 0; i--)
	{
		uint8_t r, g, b;
		
		b = (*bitmap &   0x1F) << 3;
		g = (*bitmap &  0x7E0) >> 3;
		r = (*bitmap & 0xF800) >> 8;
		
		*(dst++) = b + (b >> 5);
		*(dst++) = g + (g >> 6);
		*(dst++) = r + (r >> 5);
		
		bitmap++;
	}
}

void
rgb555_to_rgb24(uint32_t width, uint32_t height, unsigned char *dst, void *src)
{
	uint16_t *bitmap = (uint16_t *) src;
	uint32_t i = width * height;
	
	for(; i > 0; i--)
	{
		uint8_t r, g, b;
		
		b = (*bitmap &   0x1F) << 3;
		g = (*bitmap &  0x3E0) >> 2;
		r = (*bitmap & 0x7C00) >> 7;
		
		*(dst++) = b + (b >> 5);
		*(dst++) = g + (g >> 5);
		*(dst++) = r + (r >> 5);
		
		bitmap++;
	}
}

void
grey_to_rgb24(uint32_t width, uint32_t height, unsigned char *dst, void *src)
{
	uint8_t *bitmap = (uint8_t *) src;
	uint32_t i = width * height;
	
	for(; i > 0; i--)
	{
		*(dst++) = *bitmap;
		*(dst++) = *bitmap;
		*(dst++) = *(bitmap++);
	}
}
