#include <stdio.h>
#include <sys/types.h>
#include <stdlib.h>
#include <unistd.h>
#include <math.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 ])
*/

#define GAMMA_CORRECT
static double display_gamma = 2.2;

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

    for( i = width; i; i-- ) {
        if( *src == 0 ) {
            *dest++ = 0xff;
            *dest++ = 0xff;
            *dest++ = 0xff;
        } else if( *src == 0xf ) {
            double luma = *(src + 1) - 16;
            double cb = *(src + 2) - 128;
            double cr = *(src + 3) - 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;
        } else {
            double luma = *(src + 1) - 16;
            double cb = *(src + 2) - 128;
            double cr = *(src + 3) - 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 );
            double curalpha = (double) *src / 15.0;
#ifdef GAMMA_CORRECT
            double r = pow( rp / 255.0, display_gamma ) * curalpha
                       + ( 1.0 * ( 1.0 - curalpha ) );
            double g = pow( gp / 255.0, display_gamma ) * curalpha
                       + ( 1.0 * ( 1.0 - curalpha ) );
            double b = pow( bp / 255.0, display_gamma ) * curalpha
                       + ( 1.0 * ( 1.0 - curalpha ) );
            double rpf = pow( r, 1.0 / display_gamma ) * 255.0;
            double gpf = pow( g, 1.0 / display_gamma ) * 255.0;
            double bpf = pow( b, 1.0 / display_gamma ) * 255.0;
#else
            double rpf = rp * curalpha + ( 255.0 * ( 1.0 - curalpha ) );
            double gpf = gp * curalpha + ( 255.0 * ( 1.0 - curalpha ) );
            double bpf = bp * curalpha + ( 255.0 * ( 1.0 - curalpha ) );
#endif
            int rpi = rpf + 0.5;
            int gpi = gpf + 0.5;
            int bpi = bpf + 0.5;
            fprintf( stderr, "alpha went to %d %d %d from %f %f %f\n", rpi, gpi, bpi, rp, gp, bp );

            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;
        }
        src+= 4;
    }
}

int main( int argc, char **argv )
{
    int infd;
    int outfd;
    int width, height;
    unsigned char *input;
    unsigned char *output;
    int i;

    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 );

    input = malloc( width * 4 * 2 );
    output = malloc( width * 4 * 2 );

    for( i = 0; i < height; i++ ) {
        read( infd, input, width * 4 );
        ycbcr_444_packed_to_rgb32_scanline( output, input, width );
        write( outfd, output, width * 3 );
    }

    close( outfd );
    close( infd );

    return 0;
}