/*
 * 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 TRY_MV( mx, my ) \
{\
    int cost = h->dsp.sad( fenc, fref+(mx)+(my)*stride, stride );\
    COPY3_IF_LT( bcost, cost, bmx, mx, bmy, my );\
}

static int xlsc_me_fpel( xlsc_t *h, mv_t *mv, const mv_t *mvp, int nmvp )
{
    uint8_t *fenc = h->mb.fenc;
    uint8_t *fref = h->mb.fref;
    int stride = h->stride;
    int bcost;
    int bmx, bmy;
    int i;
    mv_t mv_min = { h->mb.mv_min.x>>1, h->mb.mv_min.y>>1 };
    mv_t mv_max = { h->mb.mv_max.x>>1, h->mb.mv_max.y>>1 };

    bmx = bmy = 0;
    bcost = h->dsp.sad( fenc, fref, stride );

    for( i=0; i<nmvp; i++ )
    {
        int mx = clip( mvp[i].x>>1, mv_min.x, mv_max.x );
        int my = clip( mvp[i].y>>1, mv_min.y, mv_max.y );
        TRY_MV( mx, my );
    }

    for( i=0; i<64; i++ )
    {
        uint8_t *src = fref+bmx+bmy*stride;
        int dx=0, dy=0;
        int cost[4];
        h->dsp.sad_x4( fenc, src-1, src+1, src-stride, src+stride, stride, cost );
        if(    bmx > mv_min.x && bmx < mv_max.x
            && bmy > mv_min.y && bmy < mv_max.y )
        {
            COPY2_IF_LT( bcost, cost[0], dx, -1 );
            COPY2_IF_LT( bcost, cost[1], dx, 1 );
            COPY3_IF_LT( bcost, cost[2], dx, 0, dy, -1 );
            COPY3_IF_LT( bcost, cost[3], dx, 0, dy, 1 );
        }
        else
        {
            if( bmx > mv_min.x )
                COPY2_IF_LT( bcost, cost[0], dx, -1 );
            if( bmx < mv_max.x )
                COPY2_IF_LT( bcost, cost[1], dx, 1 );
            if( bmy > mv_min.y )
                COPY3_IF_LT( bcost, cost[2], dx, 0, dy, -1 );
            if( bmy < mv_max.y )
                COPY3_IF_LT( bcost, cost[3], dx, 0, dy, 1 );
        }
        if( dx==0 && dy==0 )
            break;
        bmx += dx;
        bmy += dy;
    }

    assert(    bmx >= mv_min.x && bmx <= mv_max.x
            && bmy >= mv_min.y && bmy <= mv_max.y );

    *mv = (mv_t){ bmx*2, bmy*2 };
    return bcost;
}

int xlsc_me( xlsc_t *h, mv_t *mv, const mv_t *mvp, int nmvp )
{
    static const mv_t square[8] = {{-1,0}, {1,0}, {0,-1}, {0,1},
                                   {-1,-1}, {1,-1}, {-1,1}, {1,1}};
    int stride = h->stride;
    int bcost = xlsc_me_fpel( h, &h->mb.mv, mvp, nmvp );

    if( !h->param.umv )
        return bcost;

    mv_t bmv = h->mb.mv, omv;
    uint8_t *fenc = h->mb.fenc;
    uint8_t *src = h->mb.fref + (bmv.x>>1) + (bmv.y>>1)*stride;
    uint8_t buf_h[32*16], buf_v[16*18], buf_hv[32*18];
    int cost[8];
    int i;

    h->dsp.mc[XLSC_PIXEL_17][1]( buf_h, src-1, 32, stride, 16 );
    h->dsp.mc[XLSC_PIXEL_16][2]( buf_v, src-stride, 16, stride, 17 );
    h->dsp.mc[XLSC_PIXEL_17][3]( buf_hv, src-stride-1, 32, stride, 17 );

    bcost   = h->dsp.satd( fenc, src, stride );
    cost[0] = h->dsp.satd( fenc, buf_h,     32 );
    cost[1] = h->dsp.satd( fenc, buf_h+1,   32 );
    cost[2] = h->dsp.satd( fenc, buf_v,     16 );
    cost[3] = h->dsp.satd( fenc, buf_v+16,  16 );
    cost[4] = h->dsp.satd( fenc, buf_hv,    32 );
    cost[5] = h->dsp.satd( fenc, buf_hv+1,  32 );
    cost[6] = h->dsp.satd( fenc, buf_hv+32, 32 );
    cost[7] = h->dsp.satd( fenc, buf_hv+33, 32 );

    omv = bmv;
    for( i=0; i<8; i++ )
        COPY3_IF_LT( bcost, cost[i], bmv.x, omv.x+square[i].x, bmv.y, omv.y+square[i].y );

    mv->x = clip( bmv.x, h->mb.mv_min.x, h->mb.mv_max.x );
    mv->y = clip( bmv.y, h->mb.mv_min.y, h->mb.mv_max.y );
    return bcost;
}

void xlsc_me_rd( xlsc_t *h, int rd_cost )
{
    static const mv_t diamond[4] = {{-1,0}, {1,0}, {0,-1}, {0,1}};
    mv_t bmv = h->mb.mv;
    int bcost = rd_cost;
    int i, j;

    for( i=0; i<4; i++ )
    {
        if( bmv.x <= h->mb.mv_min.x || bmv.x >= h->mb.mv_max.x ||
            bmv.y <= h->mb.mv_min.y || bmv.y >= h->mb.mv_max.y )
            break;
        mv_t omv = bmv;
        for( j=0; j<4; j++ )
        {
            h->mb.mv.x = bmv.x + diamond[j].x;
            h->mb.mv.y = bmv.y + diamond[j].y;
            int cost = xlsc_rd_cost( h );
            COPY2_IF_LT( bcost, cost, bmv, h->mb.mv );
        }
        if( MV_EQ( omv, bmv ) )
            break;
    }

    h->mb.mv = bmv;
}

