#include "create.h"

#include <stdlib.h>

static inline void rgb_yuv(mmxnow_yuv_t* yuv, const mmxnow_rgb_t* rgb)
{
    int Y, Cb, Cr;
    uint8_t r = rgb->r;
    uint8_t g = rgb->g;
    uint8_t b = rgb->b;

    Y = 66 * r + 129 * g + 25 * b + 0x1000;
    Cb = -38 * r - 74 * g + 112 * b + 0x8000;
    Cr = 112 * r - 94 * g - 18 * b + 0x8000;
    if (Y < 0x1000) Y = 0x1000;
    else if ( Y > 0xef00) Y = 0xef00;
    if (Cb < 0x1000) Cb = 0x1000;
    else if(Cb > 0xef00) Cb = 0xef00;
    if (Cr < 0x1000) Cr = 0x1000;
    else if (Cr > 0xef00) Cr = 0xef00;

    yuv->y = (uint8_t) (Y >> 8);
    yuv->cb = (uint8_t) (Cb >> 8);
    yuv->cr = (uint8_t) (Cr >> 8);
}

static inline int rgb_y(const mmxnow_rgb_t* rgb)
{
    return (16853l * rgb->r) + (33055l * rgb->g) + (6392l * rgb->b) + 0x8000;
}

// add stride calculation

static int rgb24_to_yuv(struct mmxnow_context_t* ctx)
{
    mmxnow_image_data_t* data = (mmxnow_image_data_t*) ctx->data;

    int width = data->in.width;
    int height = data->in.height;
    int flip = data->flip;
    mmxnow_yuv_t* dest = (mmxnow_yuv_t*) data->out.plane[0] + width * height - 1;
    const mmxnow_rgb_t* src;
    int i;
    if (!flip)
	src = (const mmxnow_rgb_t*) data->in.plane[0] + width * height - 1;
    else
	src = (const mmxnow_rgb_t*) data->in.plane[0] + width - 1;

    for (i = height-1; i >= 0; i--)
    {
        int j;
	for (j = width-1; j >= 0; j--)
	{
	    rgb_yuv(dest, src);
	    src--;
	    dest--;
	}
	if (flip)
	    src += 2 * width;
    }

    return 0;
}

static int rgb24_to_yuy2(struct mmxnow_context_t* ctx)
{
    mmxnow_image_data_t* data = (mmxnow_image_data_t*) ctx->data;

    int width = data->in.width;
    int height = data->in.height;
    int flip = data->flip;
    // yuv2 - 16bit
    uint8_t* dest = (uint8_t*)data->out.plane[0] + 2 * width * height - 1;
    const mmxnow_rgb_t* src;
    int i;
    if (!flip)
	src = (const mmxnow_rgb_t*)data->in.plane[0] + width * height - 1;
    else
	src = (const mmxnow_rgb_t*)data->in.plane[0] + width - 1;

    for(i = height - 1; i >= 0; i--)
    {
        int j;
	for(j = width / 2- 1; j >= 0; j--)
	{
	    mmxnow_yuv_t yuvt;
            int y;
	    rgb_yuv(&yuvt, src);
	    *src--;
	    *dest--= yuvt.cr;
	    *dest--= yuvt.y;
	    *dest--= yuvt.cb;
	    y = rgb_y(src);
	    *dest-- = (y >> 16) + 16;
	    src--;
	}
	if (flip)
	    src += 2 * width;
    }
    return 0;
}

static int rgb24_to_yv12(struct mmxnow_context_t* ctx)
{
    mmxnow_image_data_t* data = (mmxnow_image_data_t*) ctx->data;

    int width = data->in.width;
    int height = data->in.height;
    int flip = data->flip;
    int i;
    const mmxnow_rgb_t* src1, *src2;

    //from+=2*width*height-1;
    uint8_t* dest_y1 = data->out.plane[0];
    uint8_t* dest_y2 = data->out.plane[0] + data->out.stride[0];
    //uint8_t* dest_cb = data->out.plane[0] + width * height;
    //uint8_t* dest_cr = data->out.plane[0] + 5 * width * height / 4;
    uint8_t* dest_cb = data->out.plane[1];
    uint8_t* dest_cr = data->out.plane[2];

    if (flip)
    {
	src1 = (const mmxnow_rgb_t*) data->in.plane[0] + width * (height - 1);
	src2 = (const mmxnow_rgb_t*) data->in.plane[0] + width * (height - 2);
    }
    else
    {
	src1 = (const mmxnow_rgb_t*) data->in.plane[0];
	src2 = (const mmxnow_rgb_t*) data->in.plane[0] + width;
    }


    //    uint8_t* dest;
    //    const col* src;
    //    dest=to+2*(width*height-1);
    //    if(!flip_dir)
    //	src=(const col*)from+width*height-1;
    //    else
    //	src=(const col*)from+width-1;

    for(i = height / 2 - 1; i >= 0; i--)
    {
        int j;
	for(j = width / 2 - 1; j >= 0; j--)
	{
	    mmxnow_yuv_t yuvt;
	    rgb_yuv(&yuvt, src1);
	    *src1++;
	    *dest_cb++ = yuvt.cb;
	    *dest_cr++ = yuvt.cr;
	    *dest_y1++ = yuvt.y;
	    *dest_y1++ = rgb_y(src1);
	    src1++;
	    *dest_y2++ = rgb_y(src2);
	    src2++;
	    *dest_y2++ = rgb_y(src2);
	    src2++;
	}
	dest_y1 += width;
	dest_y2 += width;
	if (flip)
	{
	    src1 -= 3*width;
	    src2 -= 3*width;
	}
	else
	{
	    src1 += width;
	    src2 += width;
	}
    }

    return 0;
}

void mmxnow_create_generic_video_rgb_yuv(mmxnow_context_t* ctx, int method)
{
    if (ctx->method == 0)
    {
	switch (method)
	{
	case MMXNOW_BGR24_TO_YUV:
	    ctx->method = rgb24_to_yuv;
	    ctx->name = "rgb24_to_yuv";
            break;
	case MMXNOW_BGR24_TO_YUY2:
	    ctx->method = rgb24_to_yuy2;
            ctx->name = "rgb24_to_yuv2";
            break;
	case MMXNOW_BGR24_TO_YV12:
	    ctx->method = rgb24_to_yv12;
            ctx->name = "rgb24_to_yv12";
            break;
	}
	if (ctx->method)
	    ctx->data = calloc(1, sizeof(mmxnow_image_data_t));
    }
}
