/*
 * Copyright (C) 2007 Loren Merritt <lorenm@u.washington.edu>
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
 */

#include "common.h"

#define SUMSUB(a,b) { int tmp = a; a += b; b = tmp - b; }
#define R(a) (((a)+0x8000)>>16)

uint64_t xlsc_ssd_plane( uint8_t *pix1, uint8_t *pix2, int stride, int width, int height )
{
    int x, y;
    uint64_t ssd = 0;
    for( y=0; y<height; y++, pix1+=stride, pix2+=stride )
    {
        int row = 0;
        for( x=0; x<width; x++ )
        {
            int d = pix1[x] - pix2[x];
            row += d*d;
        }
        ssd += row;
    }
    return ssd;
}

static int sad_16( uint8_t *fenc, uint8_t *fref, int stride )
{
    int x, y, sum = 0;
    for( y=0; y<16; y++ )
        for( x=0; x<16; x++ )
            sum += abs( fenc[x+y*FENC_STRIDE] - fref[x+y*stride] );
    return sum;
}

static void sad_x4_16( uint8_t *fenc, uint8_t *r0, uint8_t *r1, uint8_t *r2, uint8_t *r3, int stride, int res[4] )
{
    res[0] = sad_16( fenc, r0, stride );
    res[1] = sad_16( fenc, r1, stride );
    res[2] = sad_16( fenc, r2, stride );
    res[3] = sad_16( fenc, r3, stride );
}

static int ssd_16( uint8_t *fenc, uint8_t *fref, int stride )
{
    return xlsc_ssd_plane( fenc, fref, stride, 16, 16);
}

static int satd_4( uint8_t *fenc, uint8_t *fref, int stride )
{
    int16_t tmp[4][4];
    int i, sum = 0;
    for( i=0; i<4; i++, fenc+=FENC_STRIDE, fref+=stride )
    {
        int a = fenc[0] - fref[0];
        int b = fenc[1] - fref[1];
        int c = fenc[2] - fref[2];
        int d = fenc[3] - fref[3];
        SUMSUB( a, b );
        SUMSUB( c, d );
        tmp[i][0] = a+c;
        tmp[i][1] = a-c;
        tmp[i][2] = b-d;
        tmp[i][3] = b+d;
    }
    for( i=0; i<4; i++ )
    {
        int a = tmp[0][i];
        int b = tmp[1][i];
        int c = tmp[2][i];
        int d = tmp[3][i];
        SUMSUB( a, b );
        SUMSUB( c, d );
        sum += abs(a+c) + abs(a-c) + abs(b-d) + abs(b+d);
    }
    return sum;
}

static int satd_8( uint8_t *fenc, uint8_t *fref, int stride )
{
    int x, y, sum = 0;
    for( y=0; y<2; y++ )
        for( x=0; x<2; x++ )
            sum += satd_4( fenc+4*(x+y*FENC_STRIDE), fref+4*(x+y*stride), stride );
    return sum>>1;
}

static int satd_16( uint8_t *fenc, uint8_t *fref, int stride )
{
    int x, y, sum = 0;
    for( y=0; y<4; y++ )
        for( x=0; x<4; x++ )
            sum += satd_4( fenc+4*(x+y*FENC_STRIDE), fref+4*(x+y*stride), stride );
    return sum>>1;
}

static void sub_pixels_8x8( int16_t *dst, uint8_t *src1, uint8_t *src2 )
{
    int x, y;
    for( y=0; y<8; y++ )
        for( x=0; x<8; x++ )
            dst[x+y*8] = src1[x+y*FENC_STRIDE] - src2[x+y*FENC_STRIDE];
}

static void add_pixels_8x8( uint8_t *dst, int16_t *src )
{
    int x, y;
    for( y=0; y<8; y++ )
        for( x=0; x<8; x++ )
            dst[x+y*FENC_STRIDE] = clip_uint8( dst[x+y*FENC_STRIDE] + src[x+y*8] );
}

static void transpose( int16_t *dct )
{
    int x, y;
    for( y=1; y<8; y++ )
        for( x=0; x<y; x++ )
        {
            int t = dct[x+y*8];
            dct[x+y*8] = dct[y+x*8];
            dct[y+x*8] = t;
        }
}

// cos(x*pi/16)<<16
static const int c1 = 64277;
static const int c2 = 60547;
static const int c3 = 54491;
static const int c4 = 46341;
static const int c5 = 36410;
static const int c6 = 25080;
static const int c7 = 12785;

static void dct_1d( int16_t *dct )
{
    int x;
    for( x=0; x<8; x++ )
    {
        int t0 = dct[x+0*8];
        int t1 = dct[x+1*8];
        int t2 = dct[x+2*8];
        int t3 = dct[x+3*8];
        int t4 = dct[x+4*8];
        int t5 = dct[x+5*8];
        int t6 = dct[x+6*8];
        int t7 = dct[x+7*8];
        SUMSUB( t0, t7 );
        SUMSUB( t1, t6 );
        SUMSUB( t2, t5 );
        SUMSUB( t3, t4 );
        SUMSUB( t0, t3 );
        SUMSUB( t1, t2 );
        SUMSUB( t6, t5 );
        t6 = R(c4*t6);
        t5 = R(c4*t5);
        SUMSUB( t4, t5 );
        SUMSUB( t7, t6 );
        dct[x+0*8] = R(c4*(t0+t1));
        dct[x+4*8] = R(c4*(t0-t1));
        dct[x+2*8] = R(c2*t3) + R(c6*t2);
        dct[x+6*8] = R(c6*t3) - R(c2*t2);
        dct[x+1*8] = R(c1*t7) + R(c7*t4);
        dct[x+7*8] = R(c7*t7) - R(c1*t4);
        dct[x+5*8] = R(c5*t6) + R(c3*t5);
        dct[x+3*8] = R(c3*t6) - R(c5*t5);
    }
}

static void idct_1d( int16_t *dct )
{
    int x;
    for( x=0; x<8; x++ )
    {
        int y0 = dct[x+0*8];
        int y1 = dct[x+1*8];
        int y2 = dct[x+2*8];
        int y3 = dct[x+3*8];
        int y4 = dct[x+4*8];
        int y5 = dct[x+5*8];
        int y6 = dct[x+6*8];
        int y7 = dct[x+7*8];
        int t0 = c4*(y0+y4) >> 16; // int16_t ?
        int t1 = c4*(y0-y4) >> 16;
        int t2 = (c6*y2 >> 16) - (c2*y6 >> 16);
        int t3 = (c2*y2 >> 16) + (c6*y6 >> 16);
        int t4 = (c7*y1 >> 16) - (c1*y7 >> 16);
        int t7 = (c1*y1 >> 16) + (c7*y7 >> 16);
        int t5 = (c3*y5 >> 16) - (c5*y3 >> 16);
        int t6 = (c5*y5 >> 16) + (c3*y3 >> 16);
        SUMSUB( t4, t5 );
        SUMSUB( t7, t6 );
        t5 = c4*t5 >> 16;
        t6 = c4*t6 >> 16;
        SUMSUB( t0, t3 );
        SUMSUB( t1, t2 );
        SUMSUB( t6, t5 );
        dct[x+0*8] = t0+t7;
        dct[x+7*8] = t0-t7;
        dct[x+1*8] = t1+t6;
        dct[x+6*8] = t1-t6;
        dct[x+2*8] = t2+t5;
        dct[x+5*8] = t2-t5;
        dct[x+3*8] = t3+t4;
        dct[x+4*8] = t3-t4;
    }
}

static void fdct( int16_t *dct )
{
    dct_1d( dct );
    transpose( dct );
    dct_1d( dct );
}

static void idct( int16_t *dct )
{
    idct_1d( dct );
    transpose( dct );
    idct_1d( dct );
    int i;
    for( i=0; i<64; i++ )
        dct[i] = (dct[i]+8)>>4;
}

static void dct_inter( int16_t *dct, uint8_t *fenc, uint8_t *fdec )
{
    sub_pixels_8x8( dct, fenc, fdec );
    fdct( dct );
}

static void dct_intra( int16_t *dct, uint8_t *fenc, uint8_t *ignored )
{
    int x, y;
    for( y=0; y<8; y++ )
        for( x=0; x<8; x++ )
            dct[x+y*8] = fenc[x+y*FENC_STRIDE];
    fdct( dct );
    dct[0] -= 0x1000;
}

static void idct_inter( uint8_t *fdec, int16_t *dct )
{
    idct( dct );
    add_pixels_8x8( fdec, dct );
}

static void idct_intra( uint8_t *fdec, int16_t *dct )
{
    idct( dct );
    int x, y;
    for( y=0; y<8; y++ )
        for( x=0; x<8; x++ )
            fdec[x+y*FENC_STRIDE] = clip_uint8( 128 + dct[x+y*8] );
}

static void dct_init()
{
    int x0,y0,x1,y1;
    for( x0=0; x0<8; x0++ )
        for( y0=0; y0<8; y0++ )
            for( x1=0; x1<8; x1++ )
                for( y1=0; y1<8; y1++ )
                    xlsc_idct_tab[y0*8+x0][x1*8+y1] =
                        cos((x0+.5)*M_PI*x1/8)
                      * cos((y0+.5)*M_PI*y1/8)
                      * pow(2, .5*(!!x1+!!y1) - 5);
//// usage:
//  for( i=0; i<64; i++ )
//      for( j=0; j<64; j++ )
//          pixel[i] += dct[j] * xlsc_idct_tab[i][j];
}

static void quant_init( xlsc_t *h )
{
    int bias = 0x8000 - (h->param.deadzone * 0x8000 + 500) / 1000;
    int qp;
    for( qp=1; qp<128; qp++ )
    {
        xlsc_quant_tab[qp] = (0x10000 + qp*2) / (qp*4);
        h->quant_bias[qp] = (bias * qp + 0x2000) >> 14;
    }
}

static void quant( int16_t *dct, int qp, int bias )
{
    int mul = xlsc_quant_tab[qp];
    int thresh = qp*4 - bias - 1;
    int i;
    for( i=63; i>0 && (unsigned)(dct[i]+thresh) <= 2*thresh; i-- )
        dct[i] = 0;
    for( ; i>=0; i-- )
    {
        int s = dct[i] >> 15;
        int a = (dct[i] ^ s) - s;
        a = (a + bias) * mul >> 16;
        dct[i] = (a + s) ^ s;
    }
}

static void dequant( int16_t *dct, int qp )
{
    int i;
    for( i=0; i<64; i++ )
        dct[i] *= qp*4;
}

static inline void mc_avg( uint8_t *dst, uint8_t *src, int strided, int strides, int delta, int width, int height )
{
    int x, y;
    for( y=0; y<height; y++, dst+=strided, src+=strides )
        for( x=0; x<width; x++ )
            dst[x] = (src[x] + src[x+delta] + 1) >> 1;
}

#define MC_W(width)\
static void mc_copy_##width( uint8_t *dst, uint8_t *src, int strided, int strides, int height )\
{\
    int y;\
    for( y=0; y<height; y++, dst+=strided, src+=strides )\
        memcpy( dst, src, width );\
}\
static void mc_h_##width( uint8_t *dst, uint8_t *src, int strided, int strides, int height )\
{\
    mc_avg( dst, src, strided, strides, 1, width, height );\
}\
static void mc_v_##width( uint8_t *dst, uint8_t *src, int strided, int strides, int height )\
{\
    mc_avg( dst, src, strided, strides, strides, width, height );\
}\
static void mc_hv_##width( uint8_t *dst, uint8_t *src, int strided, int strides, int height )\
{\
    int x, y;\
    for( y=0; y<height; y++, dst+=strided, src+=strides )\
        for( x=0; x<width; x++ )\
            dst[x] = (((src[x] + src[x+1] + 1) >> 1)\
                    + ((src[x+strides] + src[x+1+strides] + 1) >> 1)\
                    + 1) >> 1;\
}

MC_W(17)
MC_W(16)

void xlsc_dsp_init( xlsc_t *h )
{
    h->dsp.cpu = 0;

    h->dsp.sub_dct[0] = dct_inter;
    h->dsp.sub_dct[1] = dct_intra;
    h->dsp.add_idct[0] = idct_inter;
    h->dsp.add_idct[1] = idct_intra;
    h->dsp.quant = quant;
    h->dsp.dequant = dequant;
    h->dsp.sad = sad_16;
    h->dsp.ssd = ssd_16;
    h->dsp.satd = satd_16;
    h->dsp.sad_x4 = sad_x4_16;
    h->dsp.copy = mc_copy_16;
    h->dsp.mc[XLSC_PIXEL_16][0] = mc_copy_16;
    h->dsp.mc[XLSC_PIXEL_16][1] = mc_h_16;
    h->dsp.mc[XLSC_PIXEL_16][2] = mc_v_16;
    h->dsp.mc[XLSC_PIXEL_16][3] = mc_hv_16;
    h->dsp.mc[XLSC_PIXEL_17][0] = mc_copy_17;
    h->dsp.mc[XLSC_PIXEL_17][1] = mc_h_17;
    h->dsp.mc[XLSC_PIXEL_17][2] = mc_v_17;
    h->dsp.mc[XLSC_PIXEL_17][3] = mc_hv_17;

#ifdef ARCH_X86_64
    xlsc_dsp_init_asm( h );
#endif

    dct_init();
    quant_init( h );

    if( h->param.verbose > 0 && h->dsp.cpu )
    {
        static const char *names[8] = {"MMX", "MMX2", "3DNOW", "3DNOW2", "SSE", "SSE2", "SSE3", "SSSE3"};
        int i;
        printf( "using cpu features:" );
        for( i=0; i<8; i++ )
            if( h->dsp.cpu & (1<<i) )
                printf( " %s", names[i] );
        printf( "\n" );
    }
}

#if defined(HAVE_LINALG) && defined(SYS_LINUX)
#define __USE_GNU
#include <sched.h>
int xlsc_cpu_num_processors( void )
{
    unsigned int bit;
    int np;
    cpu_set_t p_aff;
    memset( &p_aff, 0, sizeof(p_aff) );
    sched_getaffinity( 0, sizeof(p_aff), &p_aff );
    for( np = 0, bit = 0; bit < sizeof(p_aff); bit++ )
        np += (((uint8_t *)&p_aff)[bit / 8] >> (bit % 8)) & 1;
    return np;
}
#else
int xlsc_cpu_num_processors( void )
{
    return 1;
}
#endif

