/******************************************************************************
 * program:     rasimg library               raster 0.37                      *
 * function:    Usefull block operations. 				      *
 * modul:       img_tool.c                                                    *
 * licency:     GPL or LGPL                                                   *
 ******************************************************************************/
#include <stdio.h>
#ifdef _MSC_VER
#include <stdlib.h>	/* _byteswap_ushort, .... */
#endif
#include "img_tool.h"

#include "typedfs.h"


void swab16(unsigned char *block, int PixelCount)
{
#if !(defined(ST_UINT16_CPU) && defined(LD_UINT16_SWAP))
unsigned char tmp;
#endif

#ifdef NULL
 if(block==NULL) return;
#endif
 while(PixelCount-->0)
 {
#if defined(ST_UINT16_CPU) && defined(LD_UINT16_SWAP)
   ST_UINT16_CPU(block,LD_UINT16_SWAP(block));
#else
   tmp=block[1]; block[1]=block[0]; block[0]=tmp;
#endif
   block += 2;
 }
}


void swab32(unsigned char *block, int PixelCount)
{
#if !(defined(ST_UINT32_CPU) && defined(LD_UINT32_SWAP))
unsigned char tmp;
#endif

#ifdef NULL
 if(block==NULL) return;
#endif
 while(PixelCount-->0)
 {
#if defined(ST_UINT32_CPU) && defined(LD_UINT32_SWAP)
   ST_UINT32_CPU(block,LD_UINT32_SWAP(block));
#else
   tmp=block[3]; block[3]=block[0]; block[0]=tmp;
   tmp=block[2]; block[2]=block[1]; block[1]=tmp;
#endif
   block += 4;
 }
}


void swab64(unsigned char *block, int PixelCount)
{
#if !(defined(ST_UINT64_CPU) && defined(LD_UINT64_SWAP))
unsigned char tmp;
#endif

#ifdef NULL
 if(block==NULL) return;
#endif
 while(PixelCount-->0)
 {
#if defined(ST_UINT64_CPU) && defined(LD_UINT64_SWAP)
   ST_UINT64_CPU(block,LD_UINT64_SWAP(block));  
#else
   tmp=block[7]; block[7]=block[0]; block[0]=tmp;
   tmp=block[6]; block[6]=block[1]; block[1]=tmp;
   tmp=block[5]; block[5]=block[2]; block[2]=tmp;
   tmp=block[4]; block[4]=block[3]; block[3]=tmp;
#endif
   block += 8;
 }
}


/** Flip R and B coupounds inplace. 
 * param[in]	Data	Pointer to data.
 * param[in]	Size	Amount of RGB tripplets. */
void RGB_BGR(char *Data, int PixelCount)
{
char c;
  while(PixelCount-->0)
  {
    c = Data[2];
    Data[2] = *Data;
    *Data = c;
    Data += 3;
  }
}


/** Flip R and B and invert A coupounds in place.
 * param[in]	Data	Pointer to data.
 * param[in]	Size	Amount of RGB tripplets. */
void RGBA32_BGRiA32(unsigned char *Data, int PixelCount)
{
char c;
  while(PixelCount-->0)
  {
    c = Data[2];
    Data[2] = *Data;
    *Data = c;
    Data[3] = ~Data[3];
    Data += 4;
  }
}


/** Flip R and B and invert A coupounds in place.
 * param[in]	Data	Pointer to data.
 * param[in]	Size	Amount of RGB tripplets. */
void RGBA64_BGRiA64(unsigned char *Data, int PixelCount)
{
uint16_t c;
uint16_t *Data16 = (uint16_t *)Data;

  while(PixelCount-->0)
  {
    c = Data16[2];
    Data16[2] = *Data16;
    *Data16 = c;
    Data16[3] = ~Data16[3];
    Data16 += 4;
  }
}


/** Flip R and B coupounds to the different buffer. */
void RGB_BGR2(unsigned char *OutData, const unsigned char *InData, unsigned PixelCount)
{
  while(PixelCount-->0)
  {
    OutData[0] = InData[2];
    OutData[1] = InData[1];
    OutData[2] = InData[0];
    OutData += 3;
    InData += 3;
  }
}


void RGB32_BGR24(unsigned char *OutData, const unsigned char *InData, unsigned PixelCount)
{
  while(PixelCount-->0)
  {
    OutData[0] = InData[2];
    OutData[1] = InData[1];
    OutData[2] = InData[0];
    OutData += 3;
    InData += 4;
  }
}


void BGR_Gray24precise(unsigned char *OutData, const unsigned char *InData, unsigned PixelCount)
{
  while(PixelCount-->0)
  {
    OutData[2]=OutData[1]=OutData[0] = (InData[0]*4731 + InData[1]*46871 + InData[2]*13932)/65536;
    InData+=3;
    OutData+=3;
  }
}


void BGR32_Gray24(unsigned char *OutData, const unsigned char *InData, unsigned PixelCount)
{
  while(PixelCount-->0)
  {
    OutData[2]=OutData[1]=OutData[0] = (InData[0]*4731 + InData[1]*46871 + InData[2]*13932)/65536;
    InData+=4;
    OutData+=3;
  }
}


void RGB32_Gray(unsigned char *OutData, const unsigned char *InData, unsigned PixelCount)
{
  while(PixelCount-->0)
  {
    *(OutData++) = (*InData + InData[1] + InData[2]) / 3;
    InData += 4;
  }
}


void RGB24_Gray8(unsigned char *OutData, const unsigned char *InData, unsigned PixelCount)
{
  while(PixelCount-->0)
  {
    *OutData++ = (*InData+InData[1]+InData[2]) / 3;
    InData+=3;
  }
}


void RGB_Gray24(unsigned char *OutData, const unsigned char *InData, unsigned PixelCount)
{
  while(PixelCount-->0)
  {    
    *OutData = OutData[1] = OutData[2] = (*InData+InData[1]+InData[2]) / 3;
    OutData+=3;
    InData+=3;
  }
}


/** Invert block of data.
 * @param[in]	size	Block size in Bytes. */
void NotR(char *R, int size)	//R1:=not(R1)
{
  while(size>=4)
  {
    *(uint32_t*)R = ~ *(uint32_t*)R;
    R += sizeof(uint32_t);
    size -= 4;
  }
  while(size-->0)
  {
    *R = ~*R;
    R++;
  }
}


void YUV_RGB(unsigned char *OutData, const unsigned char *y, const unsigned char *u, const unsigned char *v, unsigned PixelCount)
{
int32_t X;

  while(PixelCount>=2)   //converts to RGB
  {
    const signed char U = *u++ - 128;
    const signed char V = *v++ - 128;
    int32_t Y = 256 * *(y++);

    X = (Y + 291*V) >> 8;	// Y + 1.137*V;
    if(X>255) OutData[0]=255;
    else if(X<0) OutData[0]=0;
    else OutData[0] = X;
    X = (Y -102*U - 148*V) >> 8; // Y - 0.397*U - 0.58*V;
    if(X>255) OutData[1]=255;
    else if(X<0) OutData[1]=0;
    else OutData[1] = X;
    X = (Y + 521*U) >> 8;	// Y + 2.034*U;
    if(X>255) OutData[2]=255;
    else if(X<0) OutData[2]=0;
    else OutData[2] = X;

    OutData += 3;
    Y = 256 * *(y++);

    X = (Y + 291*V) >> 8;	// Y + 1.137*V;
    if(X>255) OutData[0]=255;
    else if(X<0) OutData[0]=0;
    else OutData[0] = X;
    X = (Y -102*U - 148*V) >> 8; // Y - 0.397*U - 0.58*V;
    if(X>255) OutData[1]=255;
    else if(X<0) OutData[1]=0;
    else OutData[1] = X;
    X = (Y + 521*U) >> 8;	// Y + 2.034*U;
    if(X>255) OutData[2]=255;
    else if(X<0) OutData[2]=0;
    else OutData[2] = X;

    OutData += 3;
    PixelCount -= 2;
  }
}


void YUYV_RGB(unsigned char *OutData, const unsigned char *yuyv, unsigned PixelCount)
{
int32_t X;

  while(PixelCount>=2)
  {
    const int32_t Y = 256 * yuyv[0];
    const signed char U = yuyv[1] - 128;
    const int32_t Y2 = 256 * yuyv[2];
    const signed char V = yuyv[3] - 128;
    yuyv += 4;

    X = (Y + 291*V) >> 8;	// Y + 1.137*V;
    if(X>254) OutData[0]=255;
    else if(X<0) OutData[0]=0;
    else OutData[0] = X;
    X = (Y -102*U - 148*V) >> 8; // Y - 0.397*U - 0.58*V;
    if(X>254) OutData[1]=255;
    else if(X<0) OutData[1]=0;
    else OutData[1] = X;
    X = (Y + 521*U) >> 8;	// Y + 2.034*U;
    if(X>254) OutData[2]=255;
    else if(X<0) OutData[2]=0;
    else OutData[2] = X;

    X = (Y2 + 291*V) >> 8;	// Y + 1.137*V;
    if(X>254) OutData[3]=255;
    else if(X<0) OutData[3]=0;
    else OutData[3] = X;
    X = (Y2 -102*U - 148*V) >> 8; // Y - 0.397*U - 0.58*V;
    if(X>254) OutData[4]=255;
    else if(X<0) OutData[4]=0;
    else OutData[4] = X;
    X = (Y2 + 521*U) >> 8;	// Y + 2.034*U;
    if(X>254) OutData[5]=255;
    else if(X<0) OutData[5]=0;
    else OutData[5] = X;

    OutData += 6;
    PixelCount -= 2;
  }
}


void AYUV_BGR24(unsigned char *OutData, const unsigned char *ayuv, unsigned PixelCount)
{
int32_t X;

  while(PixelCount-- > 0)
  {
    const int32_t Y = 256 * ayuv[1];
    const signed char U = ayuv[2] - 128;
    const signed char V = ayuv[3] - 128;
    ayuv += 4;

    X = (Y + 291*V) >> 8;		// Y + 1.137*V;
    if(X>254) OutData[2]=255;
    else if(X<0) OutData[2]=0;
    else OutData[2] = X;		// B

    X = (Y -102*U - 148*V) >> 8;	// Y - 0.397*U - 0.58*V;
    if(X>254) OutData[1]=255;
    else if(X<0) OutData[1]=0;		// G
    else OutData[1] = X;

    X = (Y + 521*U) >> 8;		// Y + 2.034*U;
    if(X>254) OutData[0]=255;
    else if(X<0) OutData[0]=0;
    else OutData[0] = X;		// R

    OutData += 3;
  }
}


void UYVY_RGB(unsigned char *OutData, const unsigned char *uyvy, unsigned PixelCount)
{
int32_t X;

  while(PixelCount>=2)
  {
    const signed char U = uyvy[0] - 128;
    const int32_t Y = 256 * uyvy[1];
    const signed char V = uyvy[2] - 128;
    const int32_t Y2 = 256 * uyvy[3];
    uyvy += 4;

    X = (Y + 291*V) >> 8;	// Y + 1.137*V;
    if(X>255) OutData[0]=255;
    else if(X<0) OutData[0]=0;
    else OutData[0] = X;
    X = (Y -102*U - 148*V) >> 8; // Y - 0.397*U - 0.58*V;
    if(X>255) OutData[1]=255;
    else if(X<0) OutData[1]=0;
    else OutData[1] = X;
    X = (Y + 521*U) >> 8;	// Y + 2.034*U;
    if(X>255) OutData[2]=255;
    else if(X<0) OutData[2]=0;
    else OutData[2] = X;

    X = (Y2 + 291*V) >> 8;	// Y + 1.137*V;
    if(X>255) OutData[3]=255;
    else if(X<0) OutData[3]=0;
    else OutData[3] = X;
    X = (Y2 -102*U - 148*V) >> 8; // Y - 0.397*U - 0.58*V;
    if(X>255) OutData[4]=255;
    else if(X<0) OutData[4]=0;
    else OutData[4] = X;
    X = (Y2 + 521*U) >> 8;	// Y + 2.034*U;
    if(X>255) OutData[5]=255;
    else if(X<0) OutData[5]=0;
    else OutData[5] = X;

    OutData += 6;    
    PixelCount -= 2;
  }
}


void YVYU_RGB(unsigned char *OutData, const unsigned char *yuyv, unsigned PixelCount)
{
int32_t X;

  while(PixelCount>=2)
  {
    const int32_t Y = 256 * yuyv[0];
    const signed char V = yuyv[1] - 128;
    const int32_t Y2 = 256 * yuyv[2];
    const signed char U = yuyv[3] - 128;    
    yuyv += 4;

    X = (Y + 291*V) >> 8;	// Y + 1.137*V;
    if(X>254) OutData[0]=255;
    else if(X<=0) OutData[0]=0;
    else OutData[0] = X;
    X = (Y -102*U - 148*V) >> 8; // Y - 0.397*U - 0.58*V;
    if(X>254) OutData[1]=255;
    else if(X<=0) OutData[1]=0;
    else OutData[1] = X;
    X = (Y + 521*U) >> 8;	// Y + 2.034*U;
    if(X>254) OutData[2]=255;
    else if(X<=0) OutData[2]=0;
    else OutData[2] = X;

    X = (Y2 + 291*V) >> 8;	// Y + 1.137*V;
    if(X>254) OutData[3]=255;
    else if(X<=0) OutData[3]=0;
    else OutData[3] = X;
    X = (Y2 -102*U - 148*V) >> 8; // Y - 0.397*U - 0.58*V;
    if(X>254) OutData[4]=255;
    else if(X<=0) OutData[4]=0;
    else OutData[4] = X;
    X = (Y2 + 521*U) >> 8;	// Y + 2.034*U;
    if(X>254) OutData[5]=255;
    else if(X<=0) OutData[5]=0;
    else OutData[5] = X;

    OutData += 6;
    PixelCount -= 2;
  }
}


/// Calculate difference between 2 lines.
/// param[out]	Out	Output
/// param[in]	In	1st line. It is tollerated Out==In.
/// param[in]	In2	2nd line.
/// param[in]	SizeX	Amount of elements.
void DiffVer1_u32(uint32_t *Out, const uint32_t *In, const uint32_t *In2, int SizeX)
{
#ifdef NULL
  if(Out==NULL || In==NULL || In2==NULL) return;
#endif
  while(SizeX-- > 0)
  {
    *Out++ = (*In++)/2 + 0x80000000 - (*In2++)/2;
  }
}
