/*****************************************************************************
 *
 *  XVID MPEG-4 VIDEO CODEC
 *  - GMC interpolation module -
 *
 *  Copyright(C) 2002-2003 Pascal Massimino <skal@planet-d.net>
 *
 *  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
 *
 * $Id: gmc.c,v 1.7 2006/11/07 19:59:03 Skal Exp $
 *
 ****************************************************************************/

#include "../portab.h"
#include "../global.h"
#include "../encoder.h"
#include "gmc.h"
#include "../utils/emms.h"

#include <stdio.h>

  /* initialized by init_GMC(), for 3points */
static
void (*Predict_16x16_func)(const NEW_GMC_DATA * const This,
                           uint8_t *dst, const uint8_t *src,
                           int dststride, int srcstride, int x, int y, int rounding) = 0;
static
void (*Predict_8x8_func)(const NEW_GMC_DATA * const This,
                         uint8_t *uDst, const uint8_t *uSrc,
                         uint8_t *vDst, const uint8_t *vSrc,
                         int dststride, int srcstride, int x, int y, int rounding) = 0;

/****************************************************************************/
/* this is borrowed from   bitstream.c  until we find a common solution */
static uint32_t __inline
log2bin(uint32_t value)
{
/* Changed by Chenm001 */
#if !defined(_MSC_VER) || defined(WIN64)
  int n = 0;

  while (value) {
	value >>= 1;
	n++;
  }
  return n;
#else
  __asm {
	bsr eax, value
	inc eax
  }
#endif
}

/* 16*sizeof(int) -> 1 or 2 cachelines */
/* table lookup might be faster!  (still to be benchmarked) */

/*
static int log2bin_table[16] =
	{ 0, 1, 1, 2, 2, 2, 2, 3, 3, 3, 3, 3, 3, 3, 3, 4};
*/
/*	1  2  3  4  5  6  7  8  9 10 11 12 13 14 15 16 */

#define RDIV(a,b) (((a)>0 ? (a) + ((b)>>1) : (a) - ((b)>>1))/(b))
#define RSHIFT(a,b) ( (a)>0 ? ((a) + (1<<((b)-1)))>>(b) : ((a) + (1<<((b)-1))-1)>>(b))

#define MLT(i)  (((16-(i))<<16) + (i))
static const uint32_t MTab[16] = {
  MLT( 0), MLT( 1), MLT( 2), MLT( 3), MLT( 4), MLT( 5), MLT( 6), MLT( 7),
  MLT( 8), MLT( 9), MLT(10), MLT(11), MLT(12), MLT(13), MLT(14), MLT(15)
};
#undef MLT

/* ************************************************************
 * Pts = 2 or 3
 *
 * Warning! *src is the global frame pointer (that is: adress
 * of pixel 0,0), not the macroblock one.
 * Conversely, *dst is the macroblock top-left adress.
 */

static
void Predict_16x16_C(const NEW_GMC_DATA * const This,
                     uint8_t *dst, const uint8_t *src,
                     int dststride, int srcstride, int x, int y, int rounding)
{
	const int W = This->sW;
	const int H	= This->sH;
	const int rho = 3 - This->accuracy;
	const int Rounder = ( (1<<7) - (rounding<<(2*rho)) ) << 16;

	const int dUx = This->dU[0];
	const int dVx = This->dV[0];
	const int dUy = This->dU[1];
	const int dVy = This->dV[1];

	int Uo = This->Uo + 16*(dUy*y + dUx*x);
	int Vo = This->Vo + 16*(dVy*y + dVx*x);

	int i, j;

	dst += 16;
	for (j=16; j>0; --j) {
		int U = Uo, V = Vo;
		Uo += dUy; Vo += dVy;
		for (i=-16; i<0; ++i) {
			unsigned int f0, f1, ri = 16, rj = 16;
			int Offset;
			int u = ( U >> 16 ) << rho;
			int v = ( V >> 16 ) << rho;

			U += dUx; V += dVx;

			if (u > 0 && u <= W) { ri = MTab[u&15]; Offset = u>>4;	}
			else {
				if (u > W) Offset = W>>4;
				else Offset = 0;
				ri = MTab[0];
			}

			if (v > 0 && v <= H) { rj = MTab[v&15]; Offset += (v>>4)*srcstride; }
			else {
				if (v > H) Offset += (H>>4)*srcstride;
				rj = MTab[0];
			}

			f0	= src[Offset + 0];
			f0 |= src[Offset + 1] << 16;
			f1	= src[Offset + srcstride + 0];
			f1 |= src[Offset + srcstride + 1] << 16;
			f0 = (ri*f0)>>16;
			f1 = (ri*f1) & 0x0fff0000;
			f0 |= f1;
			f0 = (rj*f0 + Rounder) >> 24;

			dst[i] = (uint8_t)f0;
		}
		dst += dststride;
	}
}

static
void Predict_8x8_C(const NEW_GMC_DATA * const This,
                   uint8_t *uDst, const uint8_t *uSrc,
                   uint8_t *vDst, const uint8_t *vSrc,
                   int dststride, int srcstride, int x, int y, int rounding)
{
	const int W	 = This->sW >> 1;
	const int H	 = This->sH >> 1;
	const int rho = 3-This->accuracy;
	const int32_t Rounder = ( 128 - (rounding<<(2*rho)) ) << 16;

	const int32_t dUx = This->dU[0];
	const int32_t dVx = This->dV[0];
	const int32_t dUy = This->dU[1];
	const int32_t dVy = This->dV[1];

	int32_t Uo = This->Uco + 8*(dUy*y + dUx*x);
	int32_t Vo = This->Vco + 8*(dVy*y + dVx*x);

	int i, j;

	uDst += 8;
	vDst += 8;
	for (j=8; j>0; --j) {
		int32_t U = Uo, V = Vo;
		Uo += dUy; Vo += dVy;

		for (i=-8; i<0; ++i) {
			int Offset;
			uint32_t f0, f1, ri, rj;
			int32_t u, v;

			u = ( U >> 16 ) << rho;
			v = ( V >> 16 ) << rho;
			U += dUx; V += dVx;

			if (u > 0 && u <= W) {
				ri = MTab[u&15];
				Offset = u>>4;
			} else {
				if (u>W) Offset = W>>4;
				else Offset = 0;
				ri = MTab[0];
			}

			if (v > 0 && v <= H) {
				rj = MTab[v&15];
				Offset += (v>>4)*srcstride;
			} else {
				if (v>H) Offset += (H>>4)*srcstride;
				rj = MTab[0];
			}

			f0	= uSrc[Offset + 0];
			f0 |= uSrc[Offset + 1] << 16;
			f1	= uSrc[Offset + srcstride + 0];
			f1 |= uSrc[Offset + srcstride + 1] << 16;
			f0 = (ri*f0)>>16;
			f1 = (ri*f1) & 0x0fff0000;
			f0 |= f1;
			f0 = (rj*f0 + Rounder) >> 24;

			uDst[i] = (uint8_t)f0;

			f0	= vSrc[Offset + 0];
			f0 |= vSrc[Offset + 1] << 16;
			f1	= vSrc[Offset + srcstride + 0];
			f1 |= vSrc[Offset + srcstride + 1] << 16;
			f0 = (ri*f0)>>16;
			f1 = (ri*f1) & 0x0fff0000;
			f0 |= f1;
			f0 = (rj*f0 + Rounder) >> 24;

			vDst[i] = (uint8_t)f0;
		}
		uDst += dststride;
		vDst += dststride;
	}
}

static
void get_average_mv_C(const NEW_GMC_DATA * const Dsp, VECTOR * const mv,
                      int x, int y, int qpel)
{
	int i, j;
	int vx = 0, vy = 0;
	int32_t uo = Dsp->Uo + 16*(Dsp->dU[1]*y + Dsp->dU[0]*x);
	int32_t vo = Dsp->Vo + 16*(Dsp->dV[1]*y + Dsp->dV[0]*x);
	for (j=16; j>0; --j)
	{
	int32_t U, V;
	U = uo; uo += Dsp->dU[1];
	V = vo; vo += Dsp->dV[1];
	for (i=16; i>0; --i)
	{
		int32_t u,v;
		u = U >> 16; U += Dsp->dU[0]; vx += u;
		v = V >> 16; V += Dsp->dV[0]; vy += v;
	}
	}
	vx -= (256*x+120) << (5+Dsp->accuracy);	/* 120 = 15*16/2 */
	vy -= (256*y+120) << (5+Dsp->accuracy);

	mv->x = RSHIFT( vx, 8+Dsp->accuracy - qpel );
	mv->y = RSHIFT( vy, 8+Dsp->accuracy - qpel );
}

/* ************************************************************
 * simplified version for 1 warp point
 */

static
void Predict_1pt_16x16_C(const NEW_GMC_DATA * const This,
                         uint8_t *Dst, const uint8_t *Src,
                         int dststride, int srcstride, int x, int y, int rounding)
{
	const int W	 = This->sW;
	const int H	 = This->sH;
	const int rho = 3-This->accuracy;
	const int32_t Rounder = ( 128 - (rounding<<(2*rho)) ) << 16;


	int32_t uo = This->Uo + (x<<8);	 /* ((16*x)<<4) */
	int32_t vo = This->Vo + (y<<8);
	uint32_t ri = MTab[uo & 15];
	uint32_t rj = MTab[vo & 15];
	int i, j;

	int32_t Offset;
	if (vo>=(-16<<4) && vo<=H) Offset = (vo>>4)*srcstride;
	else {
		if (vo>H) Offset = ( H>>4)*srcstride;
		else Offset =-16*srcstride;
		rj = MTab[0];
	}
	if (uo>=(-16<<4) && uo<=W) Offset += (uo>>4);
	else {
		if (uo>W) Offset += (W>>4);
		else Offset -= 16;
		ri = MTab[0];
	}

	Dst += 16;

	for(j=16; j>0; --j, Offset+=srcstride-16)
	{
	for(i=-16; i<0; ++i, ++Offset)
	{
		uint32_t f0, f1;
		f0	= Src[ Offset		+0 ];
		f0 |= Src[ Offset		+1 ] << 16;
		f1	= Src[ Offset+srcstride +0 ];
		f1 |= Src[ Offset+srcstride +1 ] << 16;
		f0 = (ri*f0)>>16;
		f1 = (ri*f1) & 0x0fff0000;
		f0 |= f1;
		f0 = ( rj*f0 + Rounder ) >> 24;
		Dst[i] = (uint8_t)f0;
	}
	Dst += dststride;
	}
}

static
void Predict_1pt_8x8_C(const NEW_GMC_DATA * const This,
                       uint8_t *uDst, const uint8_t *uSrc,
                       uint8_t *vDst, const uint8_t *vSrc,
                       int dststride, int srcstride, int x, int y, int rounding)
{
	const int W	 = This->sW >> 1;
	const int H	 = This->sH >> 1;
	const int rho = 3-This->accuracy;
	const int32_t Rounder = ( 128 - (rounding<<(2*rho)) ) << 16;

	int32_t uo = This->Uco + (x<<7);
	int32_t vo = This->Vco + (y<<7);
	uint32_t rri = MTab[uo & 15];
	uint32_t rrj = MTab[vo & 15];
	int i, j;

	int32_t Offset;
	if (vo>=(-8<<4) && vo<=H) Offset = (vo>>4)*srcstride;
	else {
		if (vo>H) Offset = ( H>>4)*srcstride;
		else Offset =-8*srcstride;
		rrj = MTab[0];
	}
	if (uo>=(-8<<4) && uo<=W) Offset += (uo>>4);
	else {
		if (uo>W) Offset += ( W>>4);
		else Offset -= 8;
		rri = MTab[0];
	}

	uDst += 8;
	vDst += 8;
	for(j=8; j>0; --j, Offset+=srcstride-8)
	{
	for(i=-8; i<0; ++i, Offset++)
	{
		uint32_t f0, f1;
		f0	= uSrc[ Offset + 0 ];
		f0 |= uSrc[ Offset + 1 ] << 16;
		f1	= uSrc[ Offset + srcstride + 0 ];
		f1 |= uSrc[ Offset + srcstride + 1 ] << 16;
		f0 = (rri*f0)>>16;
		f1 = (rri*f1) & 0x0fff0000;
		f0 |= f1;
		f0 = ( rrj*f0 + Rounder ) >> 24;
		uDst[i] = (uint8_t)f0;

		f0	= vSrc[ Offset + 0 ];
		f0 |= vSrc[ Offset + 1 ] << 16;
		f1	= vSrc[ Offset + srcstride + 0 ];
		f1 |= vSrc[ Offset + srcstride + 1 ] << 16;
		f0 = (rri*f0)>>16;
		f1 = (rri*f1) & 0x0fff0000;
		f0 |= f1;
		f0 = ( rrj*f0 + Rounder ) >> 24;