#include "lpc43xx.h"
#include "FraBufQ.h"

extern DS_CamCB s_ccb;

static __inline __asm UI32 SwapBytePairSIDD(UI32 value)	// Single Instruction Dual-Data, SIDD
{
  rev16 r0, r0
  bx lr
}

signed int ATR_RAMCODE OV7670RefreshColor(void)
{
	unsigned int lcdX, lcdY;
	// use 32 bit pointer type to utilize "REV16" instruction
	volatile unsigned long *pFB;
	volatile unsigned short *pSnp, *pTmp;
	unsigned long temp;
	unsigned int xMgn, yMgn, prmsk;
	unsigned int tglBit;

	FBQ_GetProduceIndex((void**) &pFB);
	if (0 == pFB)
		return -1L;
	
	//! clip from center and display, leave the margins
	xMgn = (CAM_CAP_W - CAM_PREVIEW_W) / 2;
	yMgn = (CAM_CAP_H - CAM_PREVIEW_H) / 2;

	//! get the snap pointer to a new image, skip the area inside the margins
	prmsk = __get_PRIMASK();
	__set_PRIMASK(0);
	#if 0 == IS_TWIN_RXBUF
		tglBit = 0;
	#else
		tglBit = !s_ccb.rcvTglBit;
	#endif
	__set_PRIMASK(prmsk);
	pTmp = s_ccb.pRcvDbBuf->aa16[tglBit] + (CAM_CAP_W * yMgn + xMgn);	
	//! copy capture data to framebuffer, X and Y are swapped
	for (lcdY = 0; lcdY<CAM_PREVIEW_W; lcdY++)
	{
		pSnp = pTmp + lcdY;
		for (lcdX = 0; lcdX<CAM_PREVIEW_H ; lcdX += 4)
		{
	
			// unwind 2 times
			temp = *pSnp;		
			pSnp += CAM_CAP_W;
			temp |= ((unsigned long)(*pSnp)) << 16;
			pSnp += CAM_CAP_W;
			temp = SwapBytePairSIDD(temp);
			*pFB++ = temp;
			
			
			temp = *pSnp;		
			pSnp += CAM_CAP_W;
			temp |= ((unsigned long)(*pSnp)) << 16;
			pSnp += CAM_CAP_W;
			temp = SwapBytePairSIDD(temp);
			*pFB++ = temp;
	
		}
		pFB += (CAM_LCD_W - CAM_PREVIEW_H);
	}
	FBQ_ProduceDone();
	return 0;

}

signed int ATR_RAMCODE OV7670RefreshGray(void)
{
	unsigned int lcdX, lcdY;
	volatile unsigned short *pFB;
	volatile unsigned short *pSnp, *pTmp;
	unsigned int xMgn, yMgn, Y;
	//! clip from center and display, leave the margins
	xMgn = (CAM_CAP_W - CAM_PREVIEW_W) / 2;
	yMgn = (CAM_CAP_H - CAM_PREVIEW_H) / 2;
	FBQ_GetProduceIndex((void**) &pFB);
	if (0 == pFB)
		return -1L;
	//! get the snap pointer to a new image, skip the area inside the margins
	pTmp = s_ccb.pRcvDbBuf->aa16[!s_ccb.rcvTglBit] + CAM_CAP_W * yMgn + xMgn;

	//! copy capture data to framebuffer, X and Y are swapped
	for (lcdY = 0; lcdY<CAM_PREVIEW_W; lcdY++)
	{
		pSnp = pTmp + lcdY;
		for (lcdX = 0; lcdX<CAM_PREVIEW_H; lcdX++)
		{
	#if 1
			Y = *pSnp;
			Y = SwapBytePairSIDD(Y);
			Y = (Y >> 5) & 0x3F;
			Y = (Y>>1) | (Y<<5) | ((Y>>1) << 11);
			*pFB++ = (UI16) Y;
			pSnp += CAM_CAP_W; // to the next line (since 90 degree rotate)
	#else
	#endif
		}
		pFB += (CAM_LCD_W - CAM_PREVIEW_H);
	}
	return 0;

}
