/*---------------------------------------------------------------------------*/
/* video_format_conversion.cpp                                               */
/* copyright (c) innovaphone 2020                                            */
/*                                                                           */
/*---------------------------------------------------------------------------*/

#include <platform.h>
#include "sdk/common/interface/video_format_conversion.h"

byte Clip(int clr)
{
    return (byte)(clr < 0 ? 0 : (clr > 255 ? 255 : clr));
}

void IVideoFormatConversion::RGB24ToYUY2(unsigned char* dest, int destStride, const unsigned char* src, int srcStride, int width, int height, bool flip)
{
    dword* pSrcData = (dword*)src;
    dword* pDstData = (dword*)dest;
    dword u00, y00, v00, y10, u01, y01, v01, y11;
    dword tri0, tri1, tri2;
    int x, y;
    int b0, b1, b2, b3;
    int g0, g1, g2, g3;
    int r0, r1, r2, r3;

    for (y = 0; y < height; y++) {
        dword* pSrcPel = (dword*)pSrcData;
        dword* pDestPel = (dword*)pDstData;

        for (x = 0; x < width; x += 4) {
            // Byte order is U0 Y0 V0 Y1
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;
            tri2 = *pSrcPel++;

            b0 = ((tri0 >> 0) & 0xff);
            g0 = ((tri0 >> 8) & 0xff);
            r0 = ((tri0 >> 16) & 0xff);
            b1 = ((tri0 >> 24) & 0xff);
            g1 = ((tri1 >> 0) & 0xff);
            r1 = ((tri1 >> 8) & 0xff);
            b2 = ((tri1 >> 16) & 0xff);
            g2 = ((tri1 >> 24) & 0xff);
            r2 = ((tri2 >> 0) & 0xff);
            b3 = ((tri2 >> 8) & 0xff);
            g3 = ((tri2 >> 16) & 0xff);
            r3 = ((tri2 >> 24) & 0xff);

            y00 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            u00 = Clip(((-38 * r0 - 74 * g0 + 112 * b0 + 128) >> 8) + 128);  // U0
            v00 = Clip(((112 * r0 - 94 * g0 - 18 * b0 + 128) >> 8) + 128);  // V0
            y10 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1

            y01 = Clip(((66 * r2 + 129 * g2 + 25 * b2 + 128) >> 8) + 16);  // Y2
            u01 = Clip(((-38 * r2 - 74 * g2 + 112 * b2 + 128) >> 8) + 128);  // U1
            v01 = Clip(((112 * r2 - 94 * g2 - 18 * b2 + 128) >> 8) + 128);  // V1
            y11 = Clip(((66 * r3 + 129 * g3 + 25 * b3 + 128) >> 8) + 16);  // Y3

            *pDestPel++ = v00 << 24 | y10 << 16 | u00 << 8 | y00;
            *pDestPel++ = v01 << 24 | y11 << 16 | u01 << 8 | y01;
        }
        pSrcData += (srcStride >> 2);
        pDstData += (destStride >> 2);
    }
}

void IVideoFormatConversion::RGB32ToYUY2(unsigned char* dest, int destStride, const unsigned char* src, int srcStride, int width, int height, bool flip)
{
    dword* pSrcData = (dword*)src;
    dword* pDstData = (dword*)dest;
    dword u0, y0, v0, y1;
    dword tri0, tri1;
    int x, y;
    int b0, b1;
    int g0, g1;
    int r0, r1;

    for (y = 0; y < width; y++) {
        dword* pSrcPel = (dword*)pSrcData;
        dword* pDestPel = (dword*)pDstData;

        for (x = 0; x < height; x += 2) {
            // Byte order is U0 Y0 V0 Y1
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;

            b0 = ((tri0 >> 0) & 0xff);
            g0 = ((tri0 >> 8) & 0xff);
            r0 = ((tri0 >> 16) & 0xff);
            b1 = ((tri1 >> 0) & 0xff);
            g1 = ((tri1 >> 8) & 0xff);
            r1 = ((tri1 >> 16) & 0xff);

            y0 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            u0 = Clip(((-38 * r0 - 74 * g0 + 112 * b0 + 128) >> 8) + 128);  // U0
            v0 = Clip(((112 * r0 - 94 * g0 - 18 * b0 + 128) >> 8) + 128);  // V0
            y1 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1

            *pDestPel++ = v0 << 24 | y1 << 16 | u0 << 8 | y0;
        }
        pSrcData += (srcStride >> 2);
        pDstData += (destStride >> 2);
    }
}

void IVideoFormatConversion::NV12ToRGB32(unsigned char* dest, int destStride, const unsigned char* src, int srcStride, int width, int height, bool flip)
{
    ASSERT(0, "NV12ToRGB32 Not implemented yet!");
}

void IVideoFormatConversion::NV12ToYUY2(unsigned char* dest, int destStride, const unsigned char* src, int srcStride, int width, int height, bool flip)
{
    int x, y;
    const dword* lpBitsY = (dword*)src;
    const dword* lpBitsCbCr = (dword*)(src + (height * srcStride));
    const dword* lpBitsOut = (dword*)dest;

    for (y = 0; y < height; y += 2) {
        const dword* lpLineY0 = lpBitsY;
        const dword* lpLineY1 = (lpBitsY + (srcStride >> 2));
        const dword* lpLineCbCr = lpBitsCbCr;

        dword* lpDibLine0 = (dword*)lpBitsOut;
        dword* lpDibLine1 = (dword*)(lpBitsOut + (destStride >> 2));

        for (x = 0; x < width; x += 4) {
            dword y0 = *lpLineY0++;
            dword y1 = *lpLineY1++;
            dword cbcr = *lpLineCbCr++;

            *lpDibLine0++ = ((y0 & 0xff) << 0 | (cbcr & 0xff) << 8 | (y0 & 0xff00) << 8 | (cbcr & 0xff00) << 16);
            *lpDibLine1++ = ((y1 & 0xff) << 0 | (cbcr & 0xff) << 8 | (y1 & 0xff00) << 8 | (cbcr & 0xff00) << 16);
            *lpDibLine0++ = ((y0 & 0xff0000) >> 16 | (cbcr & 0xff0000) >> 8 | (y0 & 0xff000000) >> 8 | (cbcr & 0xff000000) << 0);
            *lpDibLine1++ = ((y1 & 0xff0000) >> 16 | (cbcr & 0xff0000) >> 8 | (y1 & 0xff000000) >> 8 | (cbcr & 0xff000000) << 0);
        }
        lpBitsY += (srcStride >> 1);
        lpBitsCbCr += (srcStride >> 2);
        lpBitsOut += (destStride >> 1);
    }
}

void IVideoFormatConversion::I420ToYUY2(unsigned char* dest, int destStride, const unsigned char* src, int srcStride, int width, int height, bool flip)
{
    int x, y;
    byte* pDstData = dest;
    const unsigned char* ybuf = src;
    const unsigned char* ubuf = src + srcStride * height;
    const unsigned char* vbuf = src + srcStride * height + ((srcStride * height) >> 2);
    int uvStride = srcStride >> 1;

    for (y = 0; y < height >> 1; y++) {
        dword* pOut = (dword*)pDstData;
        unsigned char* pYInp = (unsigned char*)ybuf;
        unsigned char* pUInp = (unsigned char*)ubuf;
        unsigned char* pVInp = (unsigned char*)vbuf;
        for (x = 0; x < width; x += 2) {
            unsigned char res0 = *pYInp++;
            unsigned char res1 = *pYInp++;
            *pOut++ = (((dword)*pVInp++) << 24) | ((dword)res1 << 16) | (((dword)*pUInp++) << 8) | (((dword)res0) & 0xff);
        }
        pDstData += destStride;
        ybuf += srcStride;

        pOut = (dword*)pDstData;
        pYInp = (unsigned char*)ybuf;
        pUInp = (unsigned char*)ubuf;
        pVInp = (unsigned char*)vbuf;
        for (x = 0; x < width; x += 2) {
            unsigned char res0 = *pYInp++;
            unsigned char res1 = *pYInp++;
            *pOut++ = (((dword)*pVInp++) << 24) | ((dword)res1 << 16) | (((dword)*pUInp++) << 8) | (((dword)res0) & 0xff);
        }
        pDstData += destStride;
        ybuf += srcStride;
        ubuf += uvStride;
        vbuf += uvStride;
    }
}

void IVideoFormatConversion::YV12ToYUY2(unsigned char* dest, int destStride, const byte* src, int srcStride, int width, int height, bool flip)
{
}

void IVideoFormatConversion::RGB24ToYUV(unsigned char* dest, int destStride, const byte* src, int srcStride, int width, int height, bool flip)
{
    const unsigned char* pInpData = (const unsigned char*)src;
    unsigned char* ybuf = dest;
    unsigned char* ubuf = dest + width * height;
    unsigned char* vbuf = dest + width * height + ((width * height) >> 2);
    int iStride = srcStride;
    int uvStride = destStride >> 1;
    dword u00, y00, v00, y10, u01, y01, v01, y11;
    dword tri0, tri1, tri2;
    int x, y;
    int b0, b1, b2, b3;
    int g0, g1, g2, g3;
    int r0, r1, r2, r3;

    for (y = 0; y < height >> 1; y++) {
        const dword* pSrcPel = (const dword*)pInpData;
        dword* pYDest = (dword*)ybuf;
        word* pUDest = (word*)ubuf;
        word* pVDest = (word*)vbuf;

        for (x = 0; x < width; x += 4) {
            // Byte order is U0 Y0 V0 Y1
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;
            tri2 = *pSrcPel++;

            b0 = ((tri0 >> 0) & 0xff);
            g0 = ((tri0 >> 8) & 0xff);
            r0 = ((tri0 >> 16) & 0xff);
            b1 = ((tri0 >> 24) & 0xff);
            g1 = ((tri1 >> 0) & 0xff);
            r1 = ((tri1 >> 8) & 0xff);
            b2 = ((tri1 >> 16) & 0xff);
            g2 = ((tri1 >> 24) & 0xff);
            r2 = ((tri2 >> 0) & 0xff);
            b3 = ((tri2 >> 8) & 0xff);
            g3 = ((tri2 >> 16) & 0xff);
            r3 = ((tri2 >> 24) & 0xff);

            y00 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            u00 = Clip(((-38 * r0 - 74 * g0 + 112 * b0 + 128) >> 8) + 128);  // U0
            v00 = Clip(((112 * r0 - 94 * g0 - 18 * b0 + 128) >> 8) + 128);  // V0
            y01 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1

            y10 = Clip(((66 * r2 + 129 * g2 + 25 * b2 + 128) >> 8) + 16);  // Y2
            u01 = Clip(((-38 * r2 - 74 * g2 + 112 * b2 + 128) >> 8) + 128);  // U1
            v01 = Clip(((112 * r2 - 94 * g2 - 18 * b2 + 128) >> 8) + 128);  // V1
            y11 = Clip(((66 * r3 + 129 * g3 + 25 * b3 + 128) >> 8) + 16);  // Y3

            *pYDest++ = y11 << 24 | y10 << 16 | y01 << 8 | y00;
            *pUDest++ = ((u01 & 0xff) << 8 | (u00 & 0xff)) & 0xffff;
            *pVDest++ = ((v01 & 0xff) << 8 | (v00 & 0xff)) & 0xffff;
        }
        pInpData += iStride;
        ybuf += destStride;

        pSrcPel = (const dword*)pInpData;
        pYDest = (dword*)ybuf;
        for (x = 0; x < width; x += 4) {
            // Byte order is U0 Y0 V0 Y1
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;
            tri2 = *pSrcPel++;

            b0 = ((tri0 >> 0) & 0xff);
            g0 = ((tri0 >> 8) & 0xff);
            r0 = ((tri0 >> 16) & 0xff);
            b1 = ((tri0 >> 24) & 0xff);
            g1 = ((tri1 >> 0) & 0xff);
            r1 = ((tri1 >> 8) & 0xff);
            b2 = ((tri1 >> 16) & 0xff);
            g2 = ((tri1 >> 24) & 0xff);
            r2 = ((tri2 >> 0) & 0xff);
            b3 = ((tri2 >> 8) & 0xff);
            g3 = ((tri2 >> 16) & 0xff);
            r3 = ((tri2 >> 24) & 0xff);

            y00 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            y01 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1
            y10 = Clip(((66 * r2 + 129 * g2 + 25 * b2 + 128) >> 8) + 16);  // Y2
            y11 = Clip(((66 * r3 + 129 * g3 + 25 * b3 + 128) >> 8) + 16);  // Y3

            *pYDest++ = y11 << 24 | y10 << 16 | y01 << 8 | y00;
        }
        pInpData += iStride;
        ybuf += destStride;
        ubuf += uvStride;
        vbuf += uvStride;
    }
}

void IVideoFormatConversion::RGB32ToYUV(unsigned char* dest, int destStride, const byte* src, int srcStride, int width, int height, bool flip)
{
    const unsigned char* pInpData = (const unsigned char*)src;
    unsigned char* ybuf = dest;
    unsigned char* ubuf = dest + width * height;
    unsigned char* vbuf = dest + width * height + ((width * height) >> 2);
    int iStride = srcStride;
    int uvStride = destStride >> 1;
    dword u00, y00, v00, y01, u01, v01, y10, y11;
    dword tri0, tri1, tri2, tri3;
    int x, y;
    int b0, b1, b2, b3;
    int g0, g1, g2, g3;
    int r0, r1, r2, r3;

    //debug->printf("IFormatConversion::RGB32ToYUV %d %d,%d,%d", destStride, srcStride, width, height);

    for (y = 0; y < height >> 1; y++) {
        const dword* pSrcPel = (const dword*)pInpData;
        dword* pYDest = (dword*)ybuf;
        word* pUDest = (word*)ubuf;
        word* pVDest = (word*)vbuf;

        for (x = 0; x < width; x += 4) {
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;
            tri2 = *pSrcPel++;
            tri3 = *pSrcPel++;

            b0 = ((tri0 >> 0) & 0xff);
            g0 = ((tri0 >> 8) & 0xff);
            r0 = ((tri0 >> 16) & 0xff);
            b1 = ((tri1 >> 0) & 0xff);
            g1 = ((tri1 >> 8) & 0xff);
            r1 = ((tri1 >> 16) & 0xff);
            b2 = ((tri2 >> 0) & 0xff);
            g2 = ((tri2 >> 8) & 0xff);
            r2 = ((tri2 >> 16) & 0xff);
            b3 = ((tri3 >> 0) & 0xff);
            g3 = ((tri3 >> 8) & 0xff);
            r3 = ((tri3 >> 16) & 0xff);

            y00 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            u00 = Clip(((-38 * r0 - 74 * g0 + 112 * b0 + 128) >> 8) + 128);  // U0
            v00 = Clip(((112 * r0 - 94 * g0 - 18 * b0 + 128) >> 8) + 128);  // V0
            y01 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1

            y10 = Clip(((66 * r2 + 129 * g2 + 25 * b2 + 128) >> 8) + 16);  // Y2
            u01 = Clip(((-38 * r2 - 74 * g2 + 112 * b2 + 128) >> 8) + 128);  // U1
            v01 = Clip(((112 * r2 - 94 * g2 - 18 * b2 + 128) >> 8) + 128);  // V1
            y11 = Clip(((66 * r3 + 129 * g3 + 25 * b3 + 128) >> 8) + 16);  // Y3

            *pYDest++ = y11 << 24 | y10 << 16 | y01 << 8 | y00;
            *pUDest++ = ((u01 & 0xff) << 8 | (u00 & 0xff)) & 0xffff;
            *pVDest++ = ((v01 & 0xff) << 8 | (v00 & 0xff)) & 0xffff;
        }
        pInpData += iStride;
        ybuf += destStride;

        pSrcPel = (const dword*)pInpData;
        pYDest = (dword*)ybuf;
        for (x = 0; x < width; x += 4) {
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;
            tri2 = *pSrcPel++;
            tri3 = *pSrcPel++;

            b0 = ((tri0 >> 0) & 0xff);
            g0 = ((tri0 >> 8) & 0xff);
            r0 = ((tri0 >> 16) & 0xff);
            b1 = ((tri1 >> 0) & 0xff);
            g1 = ((tri1 >> 8) & 0xff);
            r1 = ((tri1 >> 16) & 0xff);
            b2 = ((tri2 >> 0) & 0xff);
            g2 = ((tri2 >> 8) & 0xff);
            r2 = ((tri2 >> 16) & 0xff);
            b3 = ((tri3 >> 0) & 0xff);
            g3 = ((tri3 >> 8) & 0xff);
            r3 = ((tri3 >> 16) & 0xff);

            y00 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            y01 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1
            y10 = Clip(((66 * r2 + 129 * g2 + 25 * b2 + 128) >> 8) + 16);  // Y2
            y11 = Clip(((66 * r3 + 129 * g3 + 25 * b3 + 128) >> 8) + 16);  // Y3

            *pYDest++ = y11 << 24 | y10 << 16 | y01 << 8 | y00;
        }
        pInpData += iStride;
        ybuf += destStride;
        ubuf += uvStride;
        vbuf += uvStride;
    }
}

void IVideoFormatConversion::ARGBToYUV(unsigned char* dest, int destStride, const byte* src, int srcStride, int width, int height, bool flip)
{
    const unsigned char* pInpData = (const unsigned char*)src;
    unsigned char* ybuf = dest;
    unsigned char* ubuf = dest + width * height;
    unsigned char* vbuf = dest + width * height + ((width * height) >> 2);
    int iStride = srcStride;
    int uvStride = destStride >> 1;
    dword u00, y00, v00, y01, u01, v01, y10, y11;
    dword tri0, tri1, tri2, tri3;
    int x, y;
    int b0, b1, b2, b3;
    int g0, g1, g2, g3;
    int r0, r1, r2, r3;

    //debug->printf("IFormatConversion::RGB32ToYUV %d %d,%d,%d", destStride, srcStride, width, height);

    for (y = 0; y < height >> 1; y++) {
        const dword* pSrcPel = (const dword*)pInpData;
        dword* pYDest = (dword*)ybuf;
        word* pUDest = (word*)ubuf;
        word* pVDest = (word*)vbuf;

        for (x = 0; x < width; x += 4) {
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;
            tri2 = *pSrcPel++;
            tri3 = *pSrcPel++;

            b0 = ((tri0 >> 24) & 0xff);
            g0 = ((tri0 >> 16) & 0xff);
            r0 = ((tri0 >> 8) & 0xff);
            b1 = ((tri1 >> 24) & 0xff);
            g1 = ((tri1 >> 16) & 0xff);
            r1 = ((tri1 >> 8) & 0xff);
            b2 = ((tri2 >> 24) & 0xff);
            g2 = ((tri2 >> 16) & 0xff);
            r2 = ((tri2 >> 8) & 0xff);
            b3 = ((tri3 >> 24) & 0xff);
            g3 = ((tri3 >> 16) & 0xff);
            r3 = ((tri3 >> 8) & 0xff);

            y00 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            u00 = Clip(((-38 * r0 - 74 * g0 + 112 * b0 + 128) >> 8) + 128);  // U0
            v00 = Clip(((112 * r0 - 94 * g0 - 18 * b0 + 128) >> 8) + 128);  // V0
            y01 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1

            y10 = Clip(((66 * r2 + 129 * g2 + 25 * b2 + 128) >> 8) + 16);  // Y2
            u01 = Clip(((-38 * r2 - 74 * g2 + 112 * b2 + 128) >> 8) + 128);  // U1
            v01 = Clip(((112 * r2 - 94 * g2 - 18 * b2 + 128) >> 8) + 128);  // V1
            y11 = Clip(((66 * r3 + 129 * g3 + 25 * b3 + 128) >> 8) + 16);  // Y3

            *pYDest++ = y11 << 24 | y10 << 16 | y01 << 8 | y00;
            *pUDest++ = ((u01 & 0xff) << 8 | (u00 & 0xff)) & 0xffff;
            *pVDest++ = ((v01 & 0xff) << 8 | (v00 & 0xff)) & 0xffff;
        }
        pInpData += iStride;
        ybuf += destStride;

        pSrcPel = (const dword*)pInpData;
        pYDest = (dword*)ybuf;
        for (x = 0; x < width; x += 4) {
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;
            tri2 = *pSrcPel++;
            tri3 = *pSrcPel++;

            b0 = ((tri0 >> 24) & 0xff);
            g0 = ((tri0 >> 16) & 0xff);
            r0 = ((tri0 >> 8) & 0xff);
            b1 = ((tri1 >> 24) & 0xff);
            g1 = ((tri1 >> 16) & 0xff);
            r1 = ((tri1 >> 8) & 0xff);
            b2 = ((tri2 >> 24) & 0xff);
            g2 = ((tri2 >> 16) & 0xff);
            r2 = ((tri2 >> 8) & 0xff);
            b3 = ((tri3 >> 24) & 0xff);
            g3 = ((tri3 >> 16) & 0xff);
            r3 = ((tri3 >> 8) & 0xff);

            y00 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            y01 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1
            y10 = Clip(((66 * r2 + 129 * g2 + 25 * b2 + 128) >> 8) + 16);  // Y2
            y11 = Clip(((66 * r3 + 129 * g3 + 25 * b3 + 128) >> 8) + 16);  // Y3

            *pYDest++ = y11 << 24 | y10 << 16 | y01 << 8 | y00;
        }
        pInpData += iStride;
        ybuf += destStride;
        ubuf += uvStride;
        vbuf += uvStride;
    }
}

void IVideoFormatConversion::RGBAToYUV(unsigned char* dest, int destStride, const byte* src, int srcStride, int width, int height, bool flip)
{
    const unsigned char* pInpData = (const unsigned char*)src;
    unsigned char* ybuf = dest;
    unsigned char* ubuf = dest + width * height;
    unsigned char* vbuf = dest + width * height + ((width * height) >> 2);
    int iStride = srcStride;
    int uvStride = destStride >> 1;
    dword u00, y00, v00, y01, u01, v01, y10, y11;
    dword tri0, tri1, tri2, tri3;
    int x, y;
    int b0, b1, b2, b3;
    int g0, g1, g2, g3;
    int r0, r1, r2, r3;

    //debug->printf("IFormatConversion::RGB32ToYUV %d %d,%d,%d", destStride, srcStride, width, height);

    for (y = 0; y < height >> 1; y++) {
        const dword* pSrcPel = (const dword*)pInpData;
        dword* pYDest = (dword*)ybuf;
        word* pUDest = (word*)ubuf;
        word* pVDest = (word*)vbuf;

        for (x = 0; x < width; x += 4) {
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;
            tri2 = *pSrcPel++;
            tri3 = *pSrcPel++;

            r0 = ((tri0 >> 0) & 0xff);
            g0 = ((tri0 >> 8) & 0xff);
            b0 = ((tri0 >> 16) & 0xff);
            r1 = ((tri1 >> 0) & 0xff);
            g1 = ((tri1 >> 8) & 0xff);
            b1 = ((tri1 >> 16) & 0xff);
            r2 = ((tri2 >> 0) & 0xff);
            g2 = ((tri2 >> 8) & 0xff);
            b2 = ((tri2 >> 16) & 0xff);
            r3 = ((tri3 >> 0) & 0xff);
            g3 = ((tri3 >> 8) & 0xff);
            b3 = ((tri3 >> 16) & 0xff);

            y00 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            u00 = Clip(((-38 * r0 - 74 * g0 + 112 * b0 + 128) >> 8) + 128);  // U0
            v00 = Clip(((112 * r0 - 94 * g0 - 18 * b0 + 128) >> 8) + 128);  // V0
            y01 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1

            y10 = Clip(((66 * r2 + 129 * g2 + 25 * b2 + 128) >> 8) + 16);  // Y2
            u01 = Clip(((-38 * r2 - 74 * g2 + 112 * b2 + 128) >> 8) + 128);  // U1
            v01 = Clip(((112 * r2 - 94 * g2 - 18 * b2 + 128) >> 8) + 128);  // V1
            y11 = Clip(((66 * r3 + 129 * g3 + 25 * b3 + 128) >> 8) + 16);  // Y3

            *pYDest++ = y11 << 24 | y10 << 16 | y01 << 8 | y00;
            *pUDest++ = ((u01 & 0xff) << 8 | (u00 & 0xff)) & 0xffff;
            *pVDest++ = ((v01 & 0xff) << 8 | (v00 & 0xff)) & 0xffff;
        }
        pInpData += iStride;
        ybuf += destStride;

        pSrcPel = (const dword*)pInpData;
        pYDest = (dword*)ybuf;
        for (x = 0; x < width; x += 4) {
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;
            tri2 = *pSrcPel++;
            tri3 = *pSrcPel++;

            r0 = ((tri0 >> 0) & 0xff);
            g0 = ((tri0 >> 8) & 0xff);
            b0 = ((tri0 >> 16) & 0xff);
            r1 = ((tri1 >> 0) & 0xff);
            g1 = ((tri1 >> 8) & 0xff);
            b1 = ((tri1 >> 16) & 0xff);
            r2 = ((tri2 >> 0) & 0xff);
            g2 = ((tri2 >> 8) & 0xff);
            b2 = ((tri2 >> 16) & 0xff);
            r3 = ((tri3 >> 0) & 0xff);
            g3 = ((tri3 >> 8) & 0xff);
            b3 = ((tri3 >> 16) & 0xff);

            y00 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            y01 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1
            y10 = Clip(((66 * r2 + 129 * g2 + 25 * b2 + 128) >> 8) + 16);  // Y2
            y11 = Clip(((66 * r3 + 129 * g3 + 25 * b3 + 128) >> 8) + 16);  // Y3

            *pYDest++ = y11 << 24 | y10 << 16 | y01 << 8 | y00;
        }
        pInpData += iStride;
        ybuf += destStride;
        ubuf += uvStride;
        vbuf += uvStride;
    }
}

void IVideoFormatConversion::YUY2ToRGB32(unsigned char* dest, int destStride, const byte* src, int srcStride, int width, int height, bool flip)
{
    ASSERT(0, "YUY2ToRGB32 Not implemented yet!");
}

void IVideoFormatConversion::YUY2ToYUV(unsigned char* dest, int destStride, const byte* src, int srcStride, int width, int height, bool flip)
{
    int x, y;
    const unsigned char* pInpData = (const unsigned char*)src;
    unsigned char* ybuf = dest;
    unsigned char* ubuf = dest + width * height;
    unsigned char* vbuf = dest + width * height + ((width * height) >> 2);
    int iStride = srcStride;
    int uvStride = destStride >> 1;

    for (y = 0; y < height >> 1; y++) {
        const dword* pSrcPel = (const dword*)pInpData;
        dword* pDest = (dword*)ybuf;
        word* pUDest = (word*)ubuf;
        word* pVDest = (word*)vbuf;
        for (x = 0; x < width; x += 4) {
            dword inp0 = *pSrcPel++;
            dword inp1 = *pSrcPel++;
            unsigned char y0 = (inp0 >> 0) & 0xff;
            unsigned char y1 = (inp0 >> 16) & 0xff;
            unsigned char y2 = (inp1 >> 0) & 0xff;
            unsigned char y3 = (inp1 >> 16) & 0xff;
            unsigned char u0 = (inp0 >> 8) & 0xff;
            unsigned char v0 = (inp0 >> 24) & 0xff;
            unsigned char u1 = (inp1 >> 8) & 0xff;
            unsigned char v1 = (inp1 >> 24) & 0xff;
            *pDest++ = ((dword)y3 << 24) | ((dword)y2 << 16) | ((dword)y1 << 8) | ((dword)y0 << 0);
            *pUDest++ = u0 | (word)u1 << 8;
            *pVDest++ = v0 | (word)v1 << 8;
        }
        pInpData += iStride;
        ybuf += destStride;

        pSrcPel = (const dword*)pInpData;
        pDest = (dword*)ybuf;
        for (x = 0; x < width; x += 4) {
            dword inp0 = *pSrcPel++;
            dword inp1 = *pSrcPel++;
            unsigned char y0 = (inp0 >> 0) & 0xff;
            unsigned char y1 = (inp0 >> 16) & 0xff;
            unsigned char y2 = (inp1 >> 0) & 0xff;
            unsigned char y3 = (inp1 >> 16) & 0xff;
            *pDest++ = ((dword)y3 << 24) | ((dword)y2 << 16) | ((dword)y1 << 8) | ((dword)y0 << 0);
        }
        pInpData += iStride;
        ybuf += destStride;
        ubuf += uvStride;
        vbuf += uvStride;
    }
}

dword ConvertYUVToRGB32(int y, int u, int v)
{
    dword rgbq;

    int c = y - 16;
    int d = u - 128;
    int e = v - 128;

    byte red = Clip((298 * c + 409 * e + 128) >> 8);
    byte green = Clip((298 * c - 100 * d - 208 * e + 128) >> 8);
    byte blue = Clip((298 * c + 516 * d + 128) >> 8);

    rgbq = blue | (dword)green << 8 | (dword)red << 16;

    return rgbq;
}

void IVideoFormatConversion::YUVToRGB32(unsigned char* dest, int destStride, const unsigned char* src, int srcStride, int width, int height, bool flip)
{
    if (flip) {
        int x, y;
        unsigned char* yDest = (unsigned char*)dest;
        unsigned char* ySrc = (unsigned char*)(src + srcStride - 1);
        unsigned char* uSrc = (unsigned char*)(src + srcStride * height + (srcStride >> 1) - 1);
        unsigned char* vSrc = (unsigned char*)(src + srcStride * height + (srcStride >> 1) * (height >> 1) + (srcStride >> 1) - 1);
        int uvStride = srcStride >> 1;
        for (y = 0; y < height >> 1; y++) {
            unsigned int* pDestPel0 = (unsigned int*)yDest;
            unsigned int* pDestPel1 = (unsigned int*)(yDest + destStride);
            unsigned char* pSrcY0 = ySrc;
            unsigned char* pSrcY1 = ySrc + srcStride;
            unsigned char* pSrcU = uSrc;
            unsigned char* pSrcV = vSrc;

            for (x = 0; x < width; x += 2) {
                int y01 = (int)*pSrcY0--;
                int y00 = (int)*pSrcY0--;
                int y11 = (int)*pSrcY1--;
                int y10 = (int)*pSrcY1--;
                int u = *pSrcU--;
                int v = *pSrcV--;

                pDestPel0[x + 0] = ConvertYUVToRGB32(y00, u, v);
                pDestPel0[x + 1] = ConvertYUVToRGB32(y01, u, v);

                pDestPel1[x + 0] = ConvertYUVToRGB32(y10, u, v);
                pDestPel1[x + 1] = ConvertYUVToRGB32(y11, u, v);
            }
            ySrc += (srcStride << 1);
            uSrc += uvStride;
            vSrc += uvStride;
            yDest += (destStride << 1);
        }
    }
    else {
        int x, y;
        unsigned char* yDest = (unsigned char*)dest;
        unsigned char* ySrc = (unsigned char*)src;
        unsigned char* uSrc = (unsigned char*)(src + srcStride * height);
        unsigned char* vSrc = (unsigned char*)(src + srcStride * height + (srcStride >> 1) * (height >> 1));
        int uvStride = srcStride >> 1;
        for (y = 0; y < height >> 1; y++) {
            unsigned int* pDestPel0 = (unsigned int*)yDest;
            unsigned int* pDestPel1 = (unsigned int*)(yDest + destStride);
            unsigned char* pSrcY0 = ySrc;
            unsigned char* pSrcY1 = ySrc + srcStride;
            unsigned char* pSrcU = uSrc;
            unsigned char* pSrcV = vSrc;

            for (x = 0; x < width; x += 2) {
                int y00 = (int)*pSrcY0++;
                int y01 = (int)*pSrcY0++;
                int y10 = (int)*pSrcY1++;
                int y11 = (int)*pSrcY1++;
                int u = *pSrcU++;
                int v = *pSrcV++;

                pDestPel0[x + 0] = ConvertYUVToRGB32(y00, u, v);
                pDestPel0[x + 1] = ConvertYUVToRGB32(y01, u, v);

                pDestPel1[x + 0] = ConvertYUVToRGB32(y10, u, v);
                pDestPel1[x + 1] = ConvertYUVToRGB32(y11, u, v);
            }
            ySrc += (srcStride << 1);
            uSrc += uvStride;
            vSrc += uvStride;
            yDest += (destStride << 1);
        }
    }
}

//-------------------------------------------------------------------
// TransformImage_RGB24 
//
// RGB-24 to RGB-32
//-------------------------------------------------------------------
void IVideoFormatConversion::TransformImage_RGB24(unsigned char* pDest, int lDestStride, const unsigned char* pSrc, int lSrcStride, int dwWidthInPixels, int dwHeightInPixels, bool flip)
{
    ASSERT(0, "TransformImage_RGB24 Not implemented yet!");
#if 0
    if (flip) {
        unsigned char* pFlipSrc = (unsigned char*)(pSrc + ABS(lSrcStride) - sizeof(RGBTRIPLE));

        for (DWORD y = 0; y < dwHeightInPixels; y++) {
            RGBTRIPLE* pSrcPel = (RGBTRIPLE*)pFlipSrc;
            DWORD* pDestPel = (DWORD*)pDest;

            for (DWORD x = 0; x < dwWidthInPixels; x++) {
                RGBTRIPLE val = *pSrcPel--;

                *pDestPel++ = D3DCOLOR_XRGB(val.rgbtRed, val.rgbtGreen, val.rgbtBlue);
            }
            pFlipSrc += lSrcStride;
            pDest += lDestStride;
        }
    }
    else {
        for (DWORD y = 0; y < dwHeightInPixels; y++) {
            RGBTRIPLE* pSrcPel = (RGBTRIPLE*)pSrc;
            DWORD* pDestPel = (DWORD*)pDest;

            for (DWORD x = 0; x < dwWidthInPixels; x++) {
                pDestPel[x] = D3DCOLOR_XRGB(
                    pSrcPel[x].rgbtRed,
                    pSrcPel[x].rgbtGreen,
                    pSrcPel[x].rgbtBlue
                );
            }
            pSrc += lSrcStride;
            pDest += lDestStride;
        }
    }
#endif
}

//-------------------------------------------------------------------
// TransformImage_RGB32
//
// RGB-32 to RGB-32 
//
// Note: This function is needed to copy the image from system
// memory to the Direct3D surface.
//-------------------------------------------------------------------
void IVideoFormatConversion::TransformImage_RGB32(unsigned char* pDest, int lDestStride, const unsigned char* pSrc, int lSrcStride, int dwWidthInPixels, int dwHeightInPixels, bool flip)
{
    ASSERT(0, "TransformImage_RGB32 Not implemented yet!");
    //MFCopyImage(pDest, lDestStride, pSrc, lSrcStride, dwWidthInPixels * 4, dwHeightInPixels);
}

void IVideoScaleSample::ScaleYUY2(unsigned char* dst, int dstStride, int dstWidth, int dstHeight, const unsigned char* src, int srcStride, int srcWidth, int srcHeight)
{
    int wScaleFactor = (srcWidth / dstWidth);
    int wSrcOffset = (srcWidth - (dstWidth * wScaleFactor)) >> 1;

    int hScaleFactor = (srcHeight / dstHeight);
    int hSrcOffset = (srcHeight - (dstHeight * hScaleFactor)) >> 1;

    int x, y;
    dword* pSrcData = (dword*)(src + (hSrcOffset * srcStride));
    dword* pDstData = (dword*)dst;
    for (y = 0; y < dstHeight; y++) {
        dword* pSrcPel = (dword*)(pSrcData + (wSrcOffset * 2));
        dword* pDestPel = (dword*)pDstData;

        for (x = 0; x < dstWidth; x += 2) {
            *pDestPel++ = *pSrcPel;   // YUYV -> two samples
            pSrcPel += wScaleFactor;  // sizeof(dword)*wScaleFactor 
        }
        pSrcData += (srcStride >> 2)* hScaleFactor;
        pDstData += (dstStride >> 2);
    }
}

void IVideoFormatConversion::ScaleRGB24ToYUV(unsigned char* dest, int destStride, int destWidth, int destHeight, const byte* src, int srcStride, int width, int height)
{
    int offset = (height - destHeight) >> 1;
    const unsigned char* pInpData = (const unsigned char*)src + offset * srcStride;
    unsigned char* ybuf = dest;
    unsigned char* ubuf = dest + destWidth * destHeight;
    unsigned char* vbuf = dest + destWidth * destHeight + ((destWidth * destHeight) >> 2);
    int iStride = srcStride;
    int uvStride = destStride >> 1;
    dword u00, y00, v00, y10, u01, y01, v01, y11;
    dword tri0, tri1, tri2;
    int x, y;
    int b0, b1, b2, b3;
    int g0, g1, g2, g3;
    int r0, r1, r2, r3;

    //debug->printf("IVideoFormatConversion::ScaleRGB24ToYUV %d,(%d,%d) -> %d,(%d,%d)", srcStride, width, height, destStride, destWidth, destHeight);

    for (y = 0; y < destHeight >> 1; y++) {
        const dword* pSrcPel = (const dword*)pInpData;
        dword* pYDest = (dword*)ybuf;
        word* pUDest = (word*)ubuf;
        word* pVDest = (word*)vbuf;

        for (x = 0; x < width; x += 4) {
            // Byte order is U0 Y0 V0 Y1
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;
            tri2 = *pSrcPel++;

            b0 = ((tri0 >> 0) & 0xff);
            g0 = ((tri0 >> 8) & 0xff);
            r0 = ((tri0 >> 16) & 0xff);
            b1 = ((tri0 >> 24) & 0xff);
            g1 = ((tri1 >> 0) & 0xff);
            r1 = ((tri1 >> 8) & 0xff);
            b2 = ((tri1 >> 16) & 0xff);
            g2 = ((tri1 >> 24) & 0xff);
            r2 = ((tri2 >> 0) & 0xff);
            b3 = ((tri2 >> 8) & 0xff);
            g3 = ((tri2 >> 16) & 0xff);
            r3 = ((tri2 >> 24) & 0xff);

            y00 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            u00 = Clip(((-38 * r0 - 74 * g0 + 112 * b0 + 128) >> 8) + 128);  // U0
            v00 = Clip(((112 * r0 - 94 * g0 - 18 * b0 + 128) >> 8) + 128);  // V0
            y01 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1

            y10 = Clip(((66 * r2 + 129 * g2 + 25 * b2 + 128) >> 8) + 16);  // Y2
            u01 = Clip(((-38 * r2 - 74 * g2 + 112 * b2 + 128) >> 8) + 128);  // U1
            v01 = Clip(((112 * r2 - 94 * g2 - 18 * b2 + 128) >> 8) + 128);  // V1
            y11 = Clip(((66 * r3 + 129 * g3 + 25 * b3 + 128) >> 8) + 16);  // Y3

            *pYDest++ = y11 << 24 | y10 << 16 | y01 << 8 | y00;
            *pUDest++ = ((u01 & 0xff) << 8 | (u00 & 0xff)) & 0xffff;
            *pVDest++ = ((v01 & 0xff) << 8 | (v00 & 0xff)) & 0xffff;
        }
        pInpData += iStride;
        ybuf += destStride;

        pSrcPel = (const dword*)pInpData;
        pYDest = (dword*)ybuf;
        for (x = 0; x < width; x += 4) {
            // Byte order is U0 Y0 V0 Y1
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;
            tri2 = *pSrcPel++;

            b0 = ((tri0 >> 0) & 0xff);
            g0 = ((tri0 >> 8) & 0xff);
            r0 = ((tri0 >> 16) & 0xff);
            b1 = ((tri0 >> 24) & 0xff);
            g1 = ((tri1 >> 0) & 0xff);
            r1 = ((tri1 >> 8) & 0xff);
            b2 = ((tri1 >> 16) & 0xff);
            g2 = ((tri1 >> 24) & 0xff);
            r2 = ((tri2 >> 0) & 0xff);
            b3 = ((tri2 >> 8) & 0xff);
            g3 = ((tri2 >> 16) & 0xff);
            r3 = ((tri2 >> 24) & 0xff);

            y00 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            y01 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1
            y10 = Clip(((66 * r2 + 129 * g2 + 25 * b2 + 128) >> 8) + 16);  // Y2
            y11 = Clip(((66 * r3 + 129 * g3 + 25 * b3 + 128) >> 8) + 16);  // Y3

            *pYDest++ = y11 << 24 | y10 << 16 | y01 << 8 | y00;
        }
        pInpData += iStride;
        ybuf += destStride;
        ubuf += uvStride;
        vbuf += uvStride;
    }
}

void IVideoFormatConversion::ScaleRGB32ToYUV(unsigned char* dest, int destStride, int destWidth, int destHeight, const unsigned char * src, int srcStride, int width, int height)
{
    int offset = (height - destHeight) >> 1;
    const unsigned char* pInpData = (const unsigned char*)src + offset * srcStride;
    unsigned char* ybuf = dest;
    unsigned char* ubuf = dest + destWidth * destHeight;
    unsigned char* vbuf = dest + destWidth * destHeight + ((destWidth * destHeight) >> 2);
    int iStride = srcStride;
    int uvStride = destStride >> 1;
    dword u00, y00, v00, y01, u01, v01, y10, y11;
    dword tri0, tri1, tri2, tri3;
    int x, y;
    int b0, b1, b2, b3;
    int g0, g1, g2, g3;
    int r0, r1, r2, r3;

    //debug->printf("IVideoFormatConversion::ScaleRGB24ToYUV %d,(%d,%d) -> %d,(%d,%d)", srcStride, width, height, destStride, destWidth, destHeight);

    for (y = 0; y < destHeight >> 1; y++) {
        const dword* pSrcPel = (const dword*)pInpData;
        dword* pYDest = (dword*)ybuf;
        word* pUDest = (word*)ubuf;
        word* pVDest = (word*)vbuf;

        for (x = 0; x < width; x += 4) {
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;
            tri2 = *pSrcPel++;
            tri3 = *pSrcPel++;

            b0 = ((tri0 >> 0) & 0xff);
            g0 = ((tri0 >> 8) & 0xff);
            r0 = ((tri0 >> 16) & 0xff);
            b1 = ((tri1 >> 0) & 0xff);
            g1 = ((tri1 >> 8) & 0xff);
            r1 = ((tri1 >> 16) & 0xff);
            b2 = ((tri2 >> 0) & 0xff);
            g2 = ((tri2 >> 8) & 0xff);
            r2 = ((tri2 >> 16) & 0xff);
            b3 = ((tri3 >> 0) & 0xff);
            g3 = ((tri3 >> 8) & 0xff);
            r3 = ((tri3 >> 16) & 0xff);

            y00 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            u00 = Clip(((-38 * r0 - 74 * g0 + 112 * b0 + 128) >> 8) + 128);  // U0
            v00 = Clip(((112 * r0 - 94 * g0 - 18 * b0 + 128) >> 8) + 128);  // V0
            y01 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1

            y10 = Clip(((66 * r2 + 129 * g2 + 25 * b2 + 128) >> 8) + 16);  // Y2
            u01 = Clip(((-38 * r2 - 74 * g2 + 112 * b2 + 128) >> 8) + 128);  // U1
            v01 = Clip(((112 * r2 - 94 * g2 - 18 * b2 + 128) >> 8) + 128);  // V1
            y11 = Clip(((66 * r3 + 129 * g3 + 25 * b3 + 128) >> 8) + 16);  // Y3

            *pYDest++ = y11 << 24 | y10 << 16 | y01 << 8 | y00;
            *pUDest++ = ((u01 & 0xff) << 8 | (u00 & 0xff)) & 0xffff;
            *pVDest++ = ((v01 & 0xff) << 8 | (v00 & 0xff)) & 0xffff;
        }
        pInpData += iStride;
        ybuf += destStride;

        pSrcPel = (const dword*)pInpData;
        pYDest = (dword*)ybuf;
        for (x = 0; x < width; x += 4) {
            tri0 = *pSrcPel++;
            tri1 = *pSrcPel++;
            tri2 = *pSrcPel++;
            tri3 = *pSrcPel++;

            b0 = ((tri0 >> 0) & 0xff);
            g0 = ((tri0 >> 8) & 0xff);
            r0 = ((tri0 >> 16) & 0xff);
            b1 = ((tri1 >> 0) & 0xff);
            g1 = ((tri1 >> 8) & 0xff);
            r1 = ((tri1 >> 16) & 0xff);
            b2 = ((tri2 >> 0) & 0xff);
            g2 = ((tri2 >> 8) & 0xff);
            r2 = ((tri2 >> 16) & 0xff);
            b3 = ((tri3 >> 0) & 0xff);
            g3 = ((tri3 >> 8) & 0xff);
            r3 = ((tri3 >> 16) & 0xff);

            y00 = Clip(((66 * r0 + 129 * g0 + 25 * b0 + 128) >> 8) + 16);  // Y0
            y01 = Clip(((66 * r1 + 129 * g1 + 25 * b1 + 128) >> 8) + 16);  // Y1
            y10 = Clip(((66 * r2 + 129 * g2 + 25 * b2 + 128) >> 8) + 16);  // Y2
            y11 = Clip(((66 * r3 + 129 * g3 + 25 * b3 + 128) >> 8) + 16);  // Y3

            *pYDest++ = y11 << 24 | y10 << 16 | y01 << 8 | y00;
        }
        pInpData += iStride;
        ybuf += destStride;
        ubuf += uvStride;
        vbuf += uvStride;
    }
}

void IVideoFormatConversion::ScaleYUY2ToYUV(unsigned char* dest, int destStride, int destWidth, int destHeight, const byte* src, int srcStride, int width, int height)
{
    int x, y;
    int offset = (height - destHeight) >> 1;
    const unsigned char* pInpData = (const unsigned char*)src + offset * srcStride;
    unsigned char* ybuf = dest;
    unsigned char* ubuf = dest + destWidth * destHeight;
    unsigned char* vbuf = dest + destWidth * destHeight + ((destWidth * destHeight) >> 2);
    int iStride = srcStride;
    int uvStride = destStride >> 1;

    //debug->printf("IVideoFormatConversion::ScaleYUY2ToYUV %d,(%d,%d) -> %d,(%d,%d)", srcStride, width, height, destStride, destWidth, destHeight);

    for (y = 0; y < destHeight >> 1; y++) {
        const dword* pSrcPel = (const dword*)pInpData;
        dword* pDest = (dword*)ybuf;
        word* pUDest = (word*)ubuf;
        word* pVDest = (word*)vbuf;
        for (x = 0; x < width; x += 4) {
            dword inp0 = *pSrcPel++;
            dword inp1 = *pSrcPel++;
            unsigned char y0 = (inp0 >> 0) & 0xff;
            unsigned char y1 = (inp0 >> 16) & 0xff;
            unsigned char y2 = (inp1 >> 0) & 0xff;
            unsigned char y3 = (inp1 >> 16) & 0xff;
            unsigned char u0 = (inp0 >> 8) & 0xff;
            unsigned char v0 = (inp0 >> 24) & 0xff;
            unsigned char u1 = (inp1 >> 8) & 0xff;
            unsigned char v1 = (inp1 >> 24) & 0xff;
            *pDest++ = ((dword)y3 << 24) | ((dword)y2 << 16) | ((dword)y1 << 8) | ((dword)y0 << 0);
            *pUDest++ = u0 | (word)u1 << 8;
            *pVDest++ = v0 | (word)v1 << 8;
        }
        pInpData += iStride;
        ybuf += destStride;

        pSrcPel = (const dword*)pInpData;
        pDest = (dword*)ybuf;
        for (x = 0; x < width; x += 4) {
            dword inp0 = *pSrcPel++;
            dword inp1 = *pSrcPel++;
            unsigned char y0 = (inp0 >> 0) & 0xff;
            unsigned char y1 = (inp0 >> 16) & 0xff;
            unsigned char y2 = (inp1 >> 0) & 0xff;
            unsigned char y3 = (inp1 >> 16) & 0xff;
            *pDest++ = ((dword)y3 << 24) | ((dword)y2 << 16) | ((dword)y1 << 8) | ((dword)y0 << 0);
        }
        pInpData += iStride;
        ybuf += destStride;
        ubuf += uvStride;
        vbuf += uvStride;
    }
}

void IVideoScaleSample::ScaleRGB24(unsigned char* dst, int dstStride, int dstWidth, int dstHeight, const unsigned char* src, int srcStride, int srcWidth, int srcHeight)
{
    int wScaleFactor = (srcWidth / dstWidth);
    int wSrcOffset = (srcWidth - (dstWidth * wScaleFactor)) >> 1;

    int hScaleFactor = (srcHeight / dstHeight);
    int hSrcOffset = (srcHeight - (dstHeight * hScaleFactor)) >> 1;

    int x, y;
    char* pSrcData = (char*)(src + (hSrcOffset * srcStride));
    char* pDstData = (char*)dst;
    for (y = 0; y < dstHeight; y++) {
        char* pSrcPel = (char*)(pSrcData + (wSrcOffset * 3));
        char* pDestPel = (char*)pDstData;

        for (x = 0; x < dstWidth; x++) {
            *pDestPel++ = *(pSrcPel + 0);  // R
            *pDestPel++ = *(pSrcPel + 1);  // G
            *pDestPel++ = *(pSrcPel + 2);  // B 
            pSrcPel += (wScaleFactor * 3);
        }
        pSrcData += (srcStride * hScaleFactor);
        pDstData += dstStride;
    }
}

void IVideoScaleSample::ScaleNV12(unsigned char* dst, int dstStride, int dstWidth, int dstHeight, const unsigned char* src, int srcStride, int srcWidth, int srcHeight)
{

}

void IVideoScaleSample::ScaleI420(unsigned char* dst, int dstStride, int dstWidth, int dstHeight, const unsigned char* src, int srcStride, int srcWidth, int srcHeight)
{

}

void IVideoScaleSample::ScaleYV12(unsigned char* dst, int dstStride, int dstWidth, int dstHeight, const unsigned char* src, int srcStride, int srcWidth, int srcHeight)
{

}

void IVideoScaleSample::ScaleYUV(unsigned char* dst, int dstStride, int dstWidth, int dstHeight, const unsigned char* src, int srcStride, int srcWidth, int srcHeight)
{
    int wScaleFactor = (srcWidth / dstWidth);
    int wSrcOffset = (srcWidth - (dstWidth * wScaleFactor)) >> 1;

    int hScaleFactor = (srcHeight / dstHeight);
    int hSrcOffset = (srcHeight - (dstHeight * hScaleFactor)) >> 1;

    debug->printf("IVideoScaleSample::ScaleYUV (%d,%d) -> (%d,%d) scale=%d,%d offset=%d,%d", srcWidth, srcHeight, dstWidth, dstHeight, wScaleFactor, hScaleFactor, wSrcOffset, hSrcOffset);

    // Y
    int x, y;
    char* pSrcData = (char*)(src + (hSrcOffset * srcStride));
    char* pDstData = (char*)dst;
    for (y = 0; y < dstHeight; y++) {
        char* pSrcPel = (char*)(pSrcData + wSrcOffset);
        char* pDestPel = (char*)pDstData;

        for (x = 0; x < dstWidth; x++) {
            *pDestPel++ = *pSrcPel;  // Y
            pSrcPel += wScaleFactor;
        }

        pSrcData += (srcStride * hScaleFactor);
        pDstData += dstStride;
    }

    // U
    pSrcData = (char*)(src + (srcStride * srcHeight) + ((hSrcOffset >> 1) * (srcStride >> 1)));
    for (y = 0; y < dstHeight >> 1; y++) {
        char* pSrcPel = (char*)(pSrcData + (wSrcOffset >> 1));
        char* pDestPel = (char*)pDstData;

        for (x = 0; x < dstWidth >> 1; x++) {
            *pDestPel++ = *pSrcPel;  // U
            pSrcPel += (wScaleFactor >> 1);
        }

        pSrcData += ((srcStride >> 1) * hScaleFactor);
        pDstData += dstStride >> 1;
    }

    // V
    pSrcData = (char*)(src + (srcStride * srcHeight) + ((srcStride * srcHeight) >> 2) + ((hSrcOffset >> 1) * (srcStride >> 1)));
    for (y = 0; y < dstHeight >> 1; y++) {
        char* pSrcPel = (char*)(pSrcData + (wSrcOffset >> 1));
        char* pDestPel = (char*)pDstData;

        for (x = 0; x < dstWidth >> 1; x++) {
            *pDestPel++ = *pSrcPel;  // V
            pSrcPel += (wScaleFactor >> 1);
        }

        pSrcData += ((srcStride >> 1) * hScaleFactor);
        pDstData += dstStride >> 1;
    }
}
