#include <stdio.h>
#include <sys/types.h>
#include <stdlib.h>
#include <unistd.h>
#include <sys/stat.h>
#include <fcntl.h>

/*
[ Y' ]   [  16 ]   [  65.481 128.553  24.966 ]   [ R' ]
[ Cb ] = [ 128 ] + [ -37.797 -74.203 112.    ] * [ G' ]
[ Cr ]   [ 128 ]   [ 112.    -93.786 -18.214 ]   [ B' ]

[ R' ]   [ 0.00456621  0.          0.00625893 ]   ([ Y' ]   [  16 ])
[ G' ] = [ 0.00456621 -0.00153632 -0.00318811 ] * ([ Cb ] - [ 128 ])
[ B' ]   [ 0.00456621  0.00791071  0.         ]   ([ Cr ]   [ 128 ])

http://www.inforamp.net/~poynton/ColorFAQ.html

  R' = [ 1.1644         0    1.5960 ]   ([ Y' ]   [  16 ])
  G' = [ 1.1644   -0.3918   -0.8130 ] * ([ Cb ] - [ 128 ])
  B' = [ 1.1644    2.0172         0 ]   ([ Cr ]   [ 128 ])
*/

static void ycbcr_444_to_rgb32_scanline( unsigned char *dest,
                                         unsigned char *srcluma,
                                         unsigned char *srccb,
                                         unsigned char *srccr,
                                         int width )
{
    int i;

    for( i = width; i; i-- ) {
        double luma = *srcluma - 16;
        double cb = *srccb - 128;
        double cr = *srccr - 128;
        double rp = ( 1.1644 * luma ) + ( 1.5960 * cr );
        double gp = ( 1.1644 * luma ) - ( 0.3918 * cb ) - ( 0.8130 * cr );
        double bp = ( 1.1644 * luma ) + ( 2.0172 * cb );
        int rpi = rp + 0.5;
        int gpi = gp + 0.5;
        int bpi = bp + 0.5;
        if( rpi > 255 ) rpi = 255;
        if( gpi > 255 ) gpi = 255;
        if( bpi > 255 ) bpi = 255;
        if( rpi < 0 ) rpi = 0;
        if( gpi < 0 ) gpi = 0;
        if( bpi < 0 ) bpi = 0;
        *dest++ = rpi;
        *dest++ = gpi;
        *dest++ = bpi;
        srcluma++;
        srccb++;
        srccr++;
    }
}

static void resample_chroma_scanline_horizontal( unsigned char *dest,
                                                 unsigned char *src,
                                                 int width )
{
    int i;

    for( i = 0; i < width; i++ ) {
        *dest = *src;
        dest++;
        src++;
        if( i == width - 1 ) {
            *dest = ( ( *(src - 1) - 128 ) / 2 ) + 128;
        } else {
            int tmp = ( *(src - 1) - 128 ) + ( *src - 128 );
            tmp = tmp / 2;
            *dest = tmp + 128;
        }
        dest++;
    }
}

static void resample_chroma_scanline_vertical( unsigned char *dest,
                                               unsigned char *srctop,
                                               unsigned char *srcbot,
                                               int width )
{
    int i;

    for( i = 0; i < width; i++ ) {
        *dest = ( ( ( *srctop - 128 ) + ( *srcbot - 128 ) ) / 2 ) + 128;
        dest++;
        srctop++;
        srcbot++;
    }
}

static unsigned char black_chroma[ 800 ];

int main( int argc, char **argv )
{
    int infd;
    int outfd;
    int width, height;
    unsigned char *luma;
    unsigned char *cb;
    unsigned char *cr;
    unsigned char *tmpchromacb;
    unsigned char *tmpchromacr;
    unsigned char *tmpchromatop;
    unsigned char *tmpchromabot;
    unsigned char *output;
    int i;

    /* Call the kettle black. */
    memset( black_chroma, 128, 800 );

    infd = open( argv[ 1 ], O_RDONLY );
    if( outfd < 0 ) {
        fprintf( stderr, "Can't open %s.\n", argv[ 1 ] );
        return 1;
    }

    outfd = open( argv[ 2 ], O_WRONLY|O_CREAT, S_IREAD|S_IWRITE|S_IRGRP|S_IROTH );
    if( outfd < 0 ) {
        fprintf( stderr, "Can't open %s.\n", argv[ 2 ] );
        return 1;
    }

    read( infd, &width, sizeof( int ) );
    read( infd, &height, sizeof( int ) );

    fprintf( stderr, "Output is %dx%d resolution.\n", width, height );

    width = width * 2;
    height = height / 2;

    /* Allocate lots of memory, sometimes more than enough to be safe. */
    luma = malloc( width * height );
    cb = malloc( width/2 * height/2 );
    cr = malloc( width/2 * height/2 );
    tmpchromacb = malloc( width * 2 );
    tmpchromacr = malloc( width * 2 );
    tmpchromatop = malloc( width * 2 );
    tmpchromabot = malloc( width * 2 );
    output = malloc( width * 4 * 2 );

    /* Read in the image. */
    read( infd, luma, width * height );
    read( infd, cb, width/2 * height/2 );
    read( infd, cr, width/2 * height/2 );

    for( i = 0; i < height - 2; i += 2 ) {
        /* Top scanline we take the chroma that's there. */
        resample_chroma_scanline_horizontal( tmpchromacb, cb, width/2 );
        resample_chroma_scanline_horizontal( tmpchromacr, cr, width/2 );
        ycbcr_444_to_rgb32_scanline( output, luma,
                                     tmpchromacb, tmpchromacr, width );
        write( outfd, output, width * 3 );
        luma += width;

        /* Scanline below, we interpolate the chroma vertically. */
        resample_chroma_scanline_horizontal( tmpchromatop, cb, width/2 );
        cb += width/2;
        resample_chroma_scanline_horizontal( tmpchromabot, cb, width/2 );
        resample_chroma_scanline_vertical( tmpchromacb,
                                           tmpchromatop,
                                           tmpchromabot,
                                           width );
        resample_chroma_scanline_horizontal( tmpchromatop, cr, width/2 );
        cr += width/2;
        resample_chroma_scanline_horizontal( tmpchromabot, cr, width/2 );
        resample_chroma_scanline_vertical( tmpchromacr,
                                           tmpchromatop,
                                           tmpchromabot,
                                           width );
        ycbcr_444_to_rgb32_scanline( output, luma,
                                     tmpchromacb, tmpchromacr, width );
        write( outfd, output, width * 3 );
        luma += width;
    }

    /* For the last two scanlines, just double the chroma for now. */
    resample_chroma_scanline_horizontal( tmpchromacb, cb, width/2 );
    resample_chroma_scanline_horizontal( tmpchromacr, cr, width/2 );
    ycbcr_444_to_rgb32_scanline( output, luma,
                                 tmpchromacb, tmpchromacr, width );
    write( outfd, output, width * 3 );
    luma += width;
    ycbcr_444_to_rgb32_scanline( output, luma,
                                 tmpchromacb, tmpchromacr, width );
    write( outfd, output, width * 3 );
    luma += width;

    close( outfd );
    close( infd );

    return 0;
}