#include "H264AVCCommonLib.h"

#ifdef CFG_SHARP_INTRA

#include "H264AVCCommonLib/YuvMbBuffer.h"
#include "H264AVCCommonLib/IntYuvMbBuffer.h"
#include "H264AVCCommonLib/Upsample.h"

#include <math.h>

H264AVC_NAMESPACE_BEGIN

__inline
Upsample::Upsample()
: m_paiFilterBank        ( 0 )
, m_pabFilterLoaded      ( 0 )
{	
	init();	
}


__inline
Upsample::~Upsample()
{
	delete [] m_pabFilterLoaded;

	if( m_paiFilterBank )
	  for( int i=0; i<FILTERS_MAX*FILTER_BANKS; i++ )	
		  delete [] m_paiFilterBank[i];
	delete [] m_paiFilterBank;

}


ErrVal Upsample::create( Upsample*& rpcUpsample )
{
  rpcUpsample = new Upsample;

  ROT( NULL == rpcUpsample );

  return Err::m_nOK;
}


ErrVal Upsample::destroy()
{
  delete this;

  return Err::m_nOK;
}


__inline
ErrVal
Upsample::init( )
{

  // ALLOCATE MEMORY
  m_pabFilterLoaded		= new Bool [ FILTER_BANKS*FILTERS_MAX ];
  m_paiFilterBank		= new int* [ FILTER_BANKS*FILTERS_MAX ];
  
  // INITIALIZE MEMORY
  for( int i=0; i<FILTERS_MAX*FILTER_BANKS; i++ ){
	  m_pabFilterLoaded[i] = false;
	  m_paiFilterBank[i]   = NULL;
  }

#if FILTERS_MAX >= 1
  {
	  UP_FILTER1
	  xLoadFilter( piFilter1, 0 );
  }
#endif    

#if FILTERS_MAX >= 2
  {
	  UP_FILTER2
	  xLoadFilter( piFilter2, 1 );
  }
#endif

#if FILTERS_MAX >= 3
  {
	  UP_FILTER3
	  xLoadFilter( piFilter3, 2 );
  }
#endif
  
#if FILTERS_MAX >= 4
  {
	  UP_FILTER4
	  xLoadFilter( piFilter4, 3 );
  }
#endif  


#if FILTERS_MAX >= 5
  {
	  UP_FILTER5
	  xLoadFilter( piFilter5, 4 );
  }
#endif

#if FILTERS_MAX >= 6
  {
	  UP_FILTER6
	  xLoadFilter( piFilter6, 5 );
  }
#endif

#if FILTERS_MAX >= 7
  {
	  UP_FILTER7
	  xLoadFilter( piFilter7, 6 );
  }
#endif

#if FILTERS_MAX >= 8
  {
	  UP_FILTER8
	  xLoadFilter( piFilter8, 7 );
  }
#endif

  return Err::m_nOK;
}

ErrVal 
Upsample::upsample4x4Blk( IntYuvMbBuffer* pcPelData, 
						 IntYuvMbBufferExtension* pcSrcData, int hfilter_id, int vfilter_id ) 
{ 

    XPel*   pDst    = pcPelData->getLumBlk();	
	XPel*   pSrc    = pcSrcData->getLumBlk();
	Int     iDstStride = pcPelData->getLStride();	
	Int     iSrcStride = pcSrcData->getLStride();

	xInterpolate4x4Blk( pSrc, pDst, iSrcStride, iDstStride,
		hfilter_id, vfilter_id, FILTERBANK_4X4 );
		
	return Err::m_nOK;
}

ErrVal
Upsample::upsample4x4Blk( XPel* pSrc, int iSrcStride, XPel* pDst, 
						 int iDstStride, int hfilter_id, int vfilter_id )
{		
	xInterpolate4x4Blk( pSrc, pDst, iSrcStride, iDstStride,
		hfilter_id, vfilter_id, FILTERBANK_4X4 );
		
	return Err::m_nOK;

}

ErrVal 
Upsample::upsample8x8Blk( IntYuvMbBuffer* pcPelData, 
						 IntYuvMbBufferExtension* pcSrcData, int hfilter_id, int vfilter_id ) 
{ 

    XPel*   pDst    = pcPelData->getLumBlk();	
	XPel*   pSrc    = pcSrcData->getLumBlk();
	Int     iDstStride = pcPelData->getLStride();	
	Int     iSrcStride = pcSrcData->getLStride();

	xInterpolate8x8Blk( pSrc, pDst, iSrcStride, iDstStride,
		hfilter_id, vfilter_id );
		
	return Err::m_nOK;
}

ErrVal
Upsample::upsample8x8Blk( XPel* pSrc, int iSrcStride, XPel* pDst, 
						 int iDstStride, int hfilter_id, int vfilter_id )
{		
	xInterpolate8x8Blk( pSrc, pDst, iSrcStride, iDstStride,
		hfilter_id, vfilter_id );
		
	return Err::m_nOK;

}

ErrVal 
Upsample::upsample16x16Blk( IntYuvMbBuffer* pcPelData, 
						    IntYuvMbBufferExtension* pcSrcData, int hfilter_id, int vfilter_id ) 
{ 
	XPel*	pDst	= pcPelData->getMbLumAddr();
	XPel*   pSrc    = pcSrcData->getMbLumAddr();
	Int     iDstStride = pcPelData->getLStride();	
	Int     iSrcStride = pcSrcData->getLStride();

	xInterpolate16x16Blk( pSrc, pDst, iSrcStride, iDstStride,
		hfilter_id, vfilter_id );
		
	return Err::m_nOK;
}

ErrVal
Upsample::upsample16x16Blk( XPel* pSrc, int iSrcStride, XPel* pDst, 
						   int iDstStride, int hfilter_id, int vfilter_id )
{		
	xInterpolate16x16Blk( pSrc, pDst, iSrcStride, iDstStride,
		hfilter_id, vfilter_id );
		
	return Err::m_nOK;

}


__inline
void
Upsample::xInterpolate4x4Blk( short *piSrcBuffer, short *piDstBuffer, 
						      int iSrcStride, int iDstStride, 
							  int iHFilterID, int iVFilterID,
							  int iBankID )
{

	// DECLARATIONS
	int i, j, im2, im1, i0, ip1, ip2, ip3;
	short *piLocBuffer;

	// ALLOCATE MEMORY
	piLocBuffer = new short [15*iSrcStride];
	memcpy( piLocBuffer, piSrcBuffer-4*iSrcStride-4, 15*iSrcStride*sizeof(short) );
	piLocBuffer += 4*iSrcStride+4;

	// CONVENIENCE VARIABLES
	int *piHFilter = xGetFilter( iBankID, iHFilterID );
	int *piVFilter = xGetFilter( iBankID, iVFilterID );
	
	// VERTICAL INTERPOLATION
	for( j=-4; j<=10; j++ )
	{

		//short* piSrc = &piSrcBuffer[j];		
		short* piSrc = &piLocBuffer[j];		
		int paiWorkBuffer[16];

		//----- interpolate col -----
		for ( i=0; i<4; i+=2 )
		{
			im2     = (i-4)*iSrcStride;
			im1     = (i-2)*iSrcStride;
			i0      = i*iSrcStride;
			ip1     = (i+2)*iSrcStride;
			ip2     = (i+4)*iSrcStride;
			ip3     = (i+6)*iSrcStride;
			
			//--- even sample ---
			paiWorkBuffer[i+0] = piVFilter[13]*piSrc[ip3]
							   + piVFilter[11]*piSrc[ip2]
				  			   + piVFilter[ 9]*piSrc[ip1]		   
							   + piVFilter[ 7]*piSrc[i0 ]
							   + piVFilter[ 5]*piSrc[im1]
							   + piVFilter[ 3]*piSrc[im2];
                                    
			//--- odd sample ---
			paiWorkBuffer[i+1] = piVFilter[12]*piSrc[ip3]
							   + piVFilter[10]*piSrc[ip2]
							   + piVFilter[ 8]*piSrc[ip1]		   
							   + piVFilter[ 6]*piSrc[i0 ]
							   + piVFilter[ 4]*piSrc[im1]
							   + piVFilter[ 2]*piSrc[im2];
		}
        
        
		//----- copy back to buffer -----
		for( i = 0; i < 4; i++ )
		{
			piSrc[i*iSrcStride] = paiWorkBuffer[i];
		}
	}

	// HORIZONTAL INTERPOLATION
	for( j=0; j<4; j++ )
	{

		short* piSrc = &piLocBuffer[j*iSrcStride];		
		int paiWorkBuffer[8];

		//----- interpolate row -----
		for ( i = 0; i < 4; i+=2 )
		{
			im2     = i-4;
			im1     = i-2;
			i0      = i;
			ip1     = i+2;
			ip2     = i+4;
			ip3     = i+6;
			
			//--- even sample ---
			paiWorkBuffer[i+0] = piHFilter[13]*piSrc[ip3]
							   + piHFilter[11]*piSrc[ip2]
				  			   + piHFilter[ 9]*piSrc[ip1]		   
							   + piHFilter[ 7]*piSrc[i0 ]
							   + piHFilter[ 5]*piSrc[im1]
							   + piHFilter[ 3]*piSrc[im2];
                                    
			//--- odd sample ---
			paiWorkBuffer[i+1] = piHFilter[12]*piSrc[ip3]
							   + piHFilter[10]*piSrc[ip2]
							   + piHFilter[ 8]*piSrc[ip1]		   
							   + piHFilter[ 6]*piSrc[i0 ]
							   + piHFilter[ 4]*piSrc[im1]
							   + piHFilter[ 2]*piSrc[im2];
		}
        
        //----- copy back to buffer -----
		int div = ( piHFilter[15]*piVFilter[15] ) / 4;	
		int add = div / 2;
		for( i = 0; i < 4; i++ )
		{
			paiWorkBuffer[i] += add;
			paiWorkBuffer[i] /= div;
			paiWorkBuffer[i] = ( paiWorkBuffer[i] > 255 ) ? 255 : paiWorkBuffer[i];
			paiWorkBuffer[i] = ( paiWorkBuffer[i] < 0 ) ? 0 : paiWorkBuffer[i];
			piSrc[i] = paiWorkBuffer[i];
		}

	}

	// Copy the buffer
	for( i=0; i<4; i++ )
	{
		memcpy( &piDstBuffer[i*iDstStride], &piLocBuffer[i*iSrcStride], 4*sizeof(short) );
	}

	delete [] (piLocBuffer-4*iSrcStride-4);
	
}

__inline
void
Upsample::xInterpolate8x8Blk( short *piSrcBuffer, short *piDstBuffer, 
						      int iSrcStride, int iDstStride, 
							  int iHFilterID, int iVFilterID )
{
	xInterpolate4x4Blk( piSrcBuffer, piDstBuffer, 
		iSrcStride, iDstStride, iHFilterID, iVFilterID, FILTERBANK_8X8 );		

	xInterpolate4x4Blk( piSrcBuffer+4, piDstBuffer+4, 
		iSrcStride, iDstStride, iHFilterID, iVFilterID, FILTERBANK_8X8 );

	xInterpolate4x4Blk( piSrcBuffer+4*iSrcStride, piDstBuffer+4*iDstStride, 
		iSrcStride, iDstStride, iHFilterID, iVFilterID, FILTERBANK_8X8 );

	xInterpolate4x4Blk( piSrcBuffer+4*iSrcStride+4, piDstBuffer+4*iDstStride+4, 
		iSrcStride, iDstStride, iHFilterID, iVFilterID, FILTERBANK_8X8 );	
}

__inline
void
Upsample::xInterpolate16x16Blk( short *piSrcBuffer, short *piDstBuffer,
							    int iSrcStride, int iDstStride,
								int iHFilterID, int iVFilterID )
{
	for( UInt i=0; i<16; i+=4 )
		for( UInt j=0; j<16; j+=4 )
		{
			xInterpolate4x4Blk( piSrcBuffer+i*iSrcStride+j, piDstBuffer+i*iDstStride+j,
				iSrcStride, iDstStride, iHFilterID, iVFilterID, FILTERBANK_4X4 );
		}
}


__inline
void
Upsample::xLoadFilter( int *piFilter, int FilterID )
{
	if( FilterID >= FILTERS_MAX ){
		printf( "ERROR: INVALID LOCATION IN FILTER BANK\n" );
		exit(12);
	}

	for( int BankID=0; BankID<FILTER_BANKS; BankID++ )	
		xLoadFilter( piFilter, BankID, FilterID );
}


__inline
void
Upsample::xLoadFilter( int *piFilter, int BankID, int FilterID )
{
	if( FilterID >= FILTERS_MAX ){
		printf( "ERROR: INVALID LOCATION IN FILTER BANK\n" );
		exit(12);
	}

	if( m_paiFilterBank[BankID*FILTERS_MAX+FilterID] == NULL )
		m_paiFilterBank[BankID*FILTERS_MAX+FilterID] = new int [ FILTERS_PHASE*FILTER_TAPS ];
	
	int* piDst = m_paiFilterBank[BankID*FILTERS_MAX+FilterID];

	for( int i=0; i<16; i++ )
		piDst[i] = piFilter[i];

	// Flag that a filter exists
	m_pabFilterLoaded[BankID*FILTERS_MAX+FilterID] = true;

}

__inline
void
Upsample::xLoadFilter( int *piFilterList[ FILTERS_MAX ] )
{
	for( int i=0; i<FILTERS_MAX; i++ )
		xLoadFilter( piFilterList[i], i );
}

__inline
int*
Upsample::xGetFilter( int iBankID, int iFilterID )
{
	AOF( iFilterID < FILTERS_MAX );
	AOT( iFilterID < 0 );
	AOF( iBankID   < FILTER_BANKS );
	AOT( iBankID   < 0 );

	AOF( m_pabFilterLoaded[ iBankID*FILTERS_MAX+iFilterID ] );

	return( m_paiFilterBank[iBankID*FILTERS_MAX+iFilterID] );
}

H264AVC_NAMESPACE_END

#endif