/* ***** BEGIN LICENSE BLOCK *****
*
* $Id: wavelet_utils_mmx.cpp,v 1.8 2006/10/09 15:24:31 tjdwave Exp $ $Name: Dirac_0_7_0 $
*
* Version: MPL 1.1/GPL 2.0/LGPL 2.1
*
* The contents of this file are subject to the Mozilla Public License
* Version 1.1 (the "License");  you may not use this file except in compliance
* with the License. You may obtain a copy of the License at
* http://www.mozilla.org/MPL/
*
* Software distributed under the License is distributed on an "AS IS" basis,
* WITHOUT WARRANTY OF ANY KIND, either express or implied. See the License for
* the specific language governing rights and limitations under the License.
*
* The Original Code is BBC Research and Development code.
*
* The Initial Developer of the Original Code is the British Broadcasting
* Corporation.
* Portions created by the Initial Developer are Copyright (C) 2004.
* All Rights Reserved.
*
* Contributor(s): Anuradha Suraparaju (Original Author),
*                 Thomas Davies (wavelet_utils.cpp)
*
* Alternatively, the contents of this file may be used under the terms of
* the GNU General Public License Version 2 (the "GPL"), or the GNU Lesser
* Public License Version 2.1 (the "LGPL"), in which case the provisions of
* the GPL or the LGPL are applicable instead of those above. If you wish to
* allow use of your version of this file only under the terms of the either
* the GPL or LGPL and not to allow others to use your version of this file
* under the MPL, indicate your decision by deleting the provisions above
* and replace them with the notice and other provisions required by the GPL
* or LGPL. If you do not delete the provisions above, a recipient may use
* your version of this file under the terms of any one of the MPL, the GPL
* or the LGPL.
* ***** END LICENSE BLOCK ***** */


#ifdef HAVE_MMX
#include <libdirac_common/wavelet_utils.h>
#include <cstdlib>
#include <mmintrin.h>
using namespace dirac;

static TwoDArray<ValueType> t_temp_data;

#if 0
//Attempt1
inline void Interleave_mmx( const int xp , 
                    const int yp , 
                    const int xl , 
                    const int yl , 
                    PicArray& pic_data)
{
    const int xl2( xl>>1);
    const int yl2( yl>>1);
    const int yend( yp + yl );

    if (pic_data.LengthX() > t_temp_data.LengthX() ||
        pic_data.LengthY() > t_temp_data.LengthY())
    {
        t_temp_data.Resize(pic_data.LengthY(), pic_data.LengthX());
    }

    // Make a temporary copy of the subband
    for (int j = yp; j<yend ; j++ )
        memcpy( t_temp_data[j-yp] , pic_data[j]+xp , xl * sizeof( ValueType ) );

    int stopx = (xl2>>2)<<2;
    // Re-order to interleave
    for (int j = 0, s=yp; j<yl2 ; j++, s+=2)
    {
        ValueType *tmp1 = &t_temp_data[j][0];
        ValueType *out = &pic_data[s][xp];
        for (int i = 0 , t = xp ; i<xp+stopx ; i+=4 , t+=8)
        {
            __m64 m1 = *(__m64 *)tmp1;
            __m64 m2 = *(__m64 *)(tmp1+xl2);
            *(__m64 *)out = _mm_unpacklo_pi16 (m1, m2);    
            out+=4;
            *(__m64 *)out = _mm_unpackhi_pi16 (m1, m2);    
            out+=4;
            tmp1 += 4;
        }
        for (int i = xp+stopx , r=2*(xp+stopx) ; i<xl2 ; i++ , r += 2)
        {
            *out = *tmp1;
            ++out;
            *out = *(tmp1+xl2);
            ++out;
            ++tmp1;
        }
    }// j 
    
    for (int j = yl2, s=yp+1 ; j<yl ; j++ , s += 2)
    {
        ValueType *tmp1 = &t_temp_data[j][0];
        //ValueType *tmp2 = &t_temp_data[j][xl2];
        ValueType *out = &pic_data[s][xp];
        for (int i = 0 , t=xp; i<stopx ; i+=4 , t += 8)
        {
            __m64 m1 = *(__m64 *)tmp1;
            __m64 m2 = *(__m64 *)(tmp1+xl2);
            *(__m64 *)out = _mm_unpacklo_pi16 (m1, m2);
            out+=4;
            *(__m64 *)out = _mm_unpackhi_pi16 (m1, m2);    
            out+=4;
            tmp1 += 4;
        }
        for (int i = stopx , r=2*(xp+stopx) ; i<xl2 ; i++ , r += 2)
        {
            *out = *tmp1;
            ++out;
            *out = *(tmp1+xl2);
            ++out;
            ++tmp1;
        }
    }// j 

    _mm_empty();
}
#endif

void WaveletTransform::VHFilter::ShiftRowLeft(ValueType *row, int length, int shift)
{
    int xstop = length/4*4;
    ValueType *shift_row = row;
    for (int i = 0; i < xstop; i+=4, shift_row+=4)
        *(__m64 *)shift_row = _mm_slli_pi16 (*(__m64 *)shift_row, shift);

    // mopup
    for (int i = xstop; i < length; ++i)
        row[i] <<= shift;

    _mm_empty();
}

void WaveletTransform::VHFilter::ShiftRowRight(ValueType *row, int length, int shift)
{
    ValueType *shift_row = row;
    int round_val = 1<<(shift-1);
    __m64 mmx_round = _mm_set_pi16( round_val, round_val, round_val, round_val);

    int xstop = length/4*4;
    for (int i = 0; i < xstop; i+=4, shift_row+=4)
    {
        __m64 tmp = _mm_add_pi16 (*(__m64 *)shift_row, mmx_round);
        *(__m64 *)shift_row = _mm_srai_pi16(tmp, shift);
    }
    // mopup
    for (int i = xstop; i < length; ++i)
        row[i] = ((row[i]+round_val)>>shift);
    _mm_empty();
}

inline void Interleave_mmx( const int xp , 
                    const int yp , 
                    const int xl , 
                    const int yl , 
                    PicArray& pic_data)
{
    const int xl2( xl>>1);
    const int yl2( yl>>1);
    const int yend( yp + yl );

    if (pic_data.LengthX() > t_temp_data.LengthX() ||
        pic_data.LengthY() > t_temp_data.LengthY())
    {
        t_temp_data.Resize(pic_data.LengthY(), pic_data.LengthX());
    }

    // Make a temporary copy of the subband. We are doing a vertical
    // interleave while copying
    for (int j = yp, s=0; j<yp+yl2 ; j++, s+=2 )
        memcpy( t_temp_data[s] , pic_data[j]+xp , xl * sizeof( ValueType ) );
    for (int j = yp+yl2, s=1; j<yend ; j++, s+=2 )
        memcpy( t_temp_data[s] , pic_data[j]+xp , xl * sizeof( ValueType ) );

    int stopx = (xl2>>2)<<2;
    // Re-order to horizontally interleave
    for (int j = 0, s=yp; j<yl ; j++, ++s)
    {
        ValueType *tmp1 = &t_temp_data[j][0];
        ValueType *out = &pic_data[s][xp];
        for (int i = 0 , t = xp ; i<xp+stopx ; i+=4 , t+=8)
        {
            __m64 m1 = *(__m64 *)tmp1;
            __m64 m2 = *(__m64 *)(tmp1+xl2);
            *(__m64 *)out = _mm_unpacklo_pi16 (m1, m2);    
            out+=4;
            *(__m64 *)out = _mm_unpackhi_pi16 (m1, m2);    
            out+=4;
            tmp1 += 4;
        }
        for (int i = xp+stopx , r=2*(xp+stopx) ; i<xl2 ; i++ , r += 2)
        {
            *out = *tmp1;
            ++out;
            *out = *(tmp1+xl2);
            ++out;
            ++tmp1;
        }
    }// j 
    
    _mm_empty();
}

void WaveletTransform::VHFilterDD9_3::Synth(const int xp , 
                                                const int yp , 
                                                const int xl , 
                                                const int yl , 
                                                PicArray& pic_data)
{
    int i, j;
    const int xend( xp+xl );
    const int yend( yp+yl );
    const int ymid = yp+yl/2;

    PredictStepShift<2> predict;
    __m64 pred_round = _mm_set_pi16 (1<<(2-1), 1<<(2-1), 1<<(2-1), 1<<(2-1));
    
    int xstop = xp + (xl>>2)<<2;

    // First lifting stage
    // Top edge
    ValueType *in1 =  pic_data[ymid];
    ValueType *in2 =  pic_data[ymid];
    ValueType *out = pic_data[yp];
       for ( i = xp ; i < xstop ; i+=4 )
    {
        // tmp = val + val2
        __m64 tmp = _mm_add_pi16 (*(__m64 *)in1, *(__m64 *)in2);
        // unbiased rounding tmp = (tmp + 1<<(shift-1))>>shift
        tmp = _mm_add_pi16(tmp, pred_round);
        tmp = _mm_srai_pi16(tmp, 2);
        // in_val -= tmp;
        *(__m64 *)out = _mm_sub_pi16 (*(__m64*)out, tmp);
        out += 4;
        in1 += 4;
        in2 += 4;
    }

    // Middle bit
       for ( j=1 ; j < yl/2 ; ++j )
    {
        in1 =  pic_data[ymid+j-1];
        in2 =  pic_data[ymid+j];
        out = pic_data[yp+j];
           for ( i = xp ; i < xstop ; i+=4 )
        {
            // tmp = val + val2
            __m64 tmp = _mm_add_pi16 (*(__m64 *)in1, *(__m64 *)in2);
            // unbiased rounding tmp = (tmp + 1<<(shift-1))>>shift
            tmp = _mm_add_pi16(tmp, pred_round);
            tmp = _mm_srai_pi16(tmp, 2);
            // in_val -= tmp;
            *(__m64 *)out = _mm_sub_pi16 (*(__m64*)out, tmp);
            out += 4;
            in1 += 4;
            in2 += 4;
        }
    }

    // Mopup
    if (xstop != xend)
    {
        for ( i = xstop ; i < xend ; i++)
        {
            predict.Filter( pic_data[yp][i] , pic_data[ymid][i] , pic_data[ymid][i] );
        }// i

        for ( j=1 ; j < yl/2 ; ++j )
        {
            for ( i = xstop ; i < xend ; i++)
            {
                predict.Filter( pic_data[yp+j][i] , pic_data[ymid+j-1][i] , pic_data[ymid+j][i] );
            }// i
        }// j
    }
 
    // Second lifting stage
    UpdateStepFourTap< 4 , 9 , -1> update;
    // rounding factor for update step
    __m64 update_round = _mm_set_pi32 (1<<(4-1), 1<<(4-1));
    // top edge
    in1 = pic_data[yp];
    in2 = pic_data[yp+1];
    ValueType *in3 = pic_data[yp];
    ValueType *in4 = pic_data[yp+2];
    out = pic_data[ymid];
    __m64 tap1 = _mm_set_pi16 (9, 9, 9, 9);
    __m64 tap2 = _mm_set_pi16 (-1, -1, -1, -1);
  
    for ( i = xp ; i < xstop ; i+=4 )
    {
        __m64 val1, val2, val3, val4, tmp1, tmp2;

        val1 = _mm_add_pi16(*(__m64*)in1, *(__m64*)in2);
        tmp1 = _mm_mullo_pi16 (val1, tap1);
        tmp2 = _mm_mulhi_pi16 (val1, tap1);
        val3 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val1 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val2 = _mm_add_pi16(*(__m64*)in3, *(__m64*)in4);
        tmp1 = _mm_mullo_pi16 (val2, tap2);
        tmp2 = _mm_mulhi_pi16 (val2, tap2);
        val4 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val2 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val1 = _mm_add_pi32 (val1, val2);
        val3 = _mm_add_pi32 (val3, val4);
        val1 = _mm_add_pi32 (val1, update_round);
        val3 = _mm_add_pi32 (val3, update_round);
        val1 = _mm_srai_pi32(val1, 4);
        val3 = _mm_srai_pi32(val3, 4);
        val1 = _mm_packs_pi32 (val1, val3);
        
        *(__m64*)out = _mm_add_pi16 (*(__m64*)out,val1);
        in1 +=4;
        in2 +=4;
        in3 +=4;
        in4 +=4;
        out +=4;
    }

       // middle bit
       for ( j=1 ; j < yl/2 - 2 ; ++j)
       {
        in1 = pic_data[yp+j];
        in2 = pic_data[yp+j+1];
        in3 = pic_data[yp+j-1];
        in4 = pic_data[yp+j+2];
        out = pic_data[ymid+j];
        for ( i = xp ; i < xstop ; i+=4)
        {
            __m64 val1, val2, val3, val4, tmp1, tmp2;

            val1 = _mm_add_pi16(*(__m64*)in1, *(__m64*)in2);
            tmp1 = _mm_mullo_pi16 (val1, tap1);
            tmp2 = _mm_mulhi_pi16 (val1, tap1);
            val3 = _mm_unpackhi_pi16 (tmp1, tmp2);
            val1 = _mm_unpacklo_pi16 (tmp1, tmp2);

            val2 = _mm_add_pi16(*(__m64*)in3, *(__m64*)in4);
            tmp1 = _mm_mullo_pi16 (val2, tap2);
            tmp2 = _mm_mulhi_pi16 (val2, tap2);
            val4 = _mm_unpackhi_pi16 (tmp1, tmp2);
            val2 = _mm_unpacklo_pi16 (tmp1, tmp2);

            val1 = _mm_add_pi32 (val1, val2);
            val3 = _mm_add_pi32 (val3, val4);
            val1 = _mm_add_pi32 (val1, update_round);
            val3 = _mm_add_pi32 (val3, update_round);
            val1 = _mm_srai_pi32(val1, 4);
            val3 = _mm_srai_pi32(val3, 4);
            val1 = _mm_packs_pi32 (val1, val3);
        
            *(__m64*)out = _mm_add_pi16 (*(__m64*)out,val1);
            in1 +=4;
            in2 +=4;
            in3 +=4;
            in4 +=4;
            out +=4;
           }// i
       }// k
   
       // bottom edge
    in1 = pic_data[ymid-2];
    in2 = pic_data[ymid-1];
    in3 = pic_data[ymid-3];
    in4 = pic_data[ymid-1];
    out = pic_data[yend-2];
    for ( i = xp ; i < xstop ; i+=4)
    {
        __m64 val1, val2, val3, val4, tmp1, tmp2;

        val1 = _mm_add_pi16(*(__m64*)in1, *(__m64*)in2);
        tmp1 = _mm_mullo_pi16 (val1, tap1);
        tmp2 = _mm_mulhi_pi16 (val1, tap1);
        val3 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val1 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val2 = _mm_add_pi16(*(__m64*)in3, *(__m64*)in4);
        tmp1 = _mm_mullo_pi16 (val2, tap2);
        tmp2 = _mm_mulhi_pi16 (val2, tap2);
        val4 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val2 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val1 = _mm_add_pi32 (val1, val2);
        val3 = _mm_add_pi32 (val3, val4);
        val1 = _mm_add_pi32 (val1, update_round);
        val3 = _mm_add_pi32 (val3, update_round);
        val1 = _mm_srai_pi32(val1, 4);
        val3 = _mm_srai_pi32(val3, 4);
        val1 = _mm_packs_pi32 (val1, val3);
       
        *(__m64*)out = _mm_add_pi16 (*(__m64*)out,val1);
        in1 +=4;
        in2 +=4;
        in3 +=4;
        in4 +=4;
        out +=4;
    }
        
    in1 = pic_data[ymid-1];
    in2 = pic_data[ymid-1];
    in3 = pic_data[ymid-2];
    in4 = pic_data[ymid-1];
    out = pic_data[yend-1];
    for ( i = xp ; i < xstop ; i+=4)
    {
        __m64 val1, val2, val3, val4, tmp1, tmp2;

        val1 = _mm_add_pi16(*(__m64*)in1, *(__m64*)in2);
        tmp1 = _mm_mullo_pi16 (val1, tap1);
        tmp2 = _mm_mulhi_pi16 (val1, tap1);
        val3 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val1 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val2 = _mm_add_pi16(*(__m64*)in3, *(__m64*)in4);
        tmp1 = _mm_mullo_pi16 (val2, tap2);
        tmp2 = _mm_mulhi_pi16 (val2, tap2);
        val4 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val2 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val1 = _mm_add_pi32 (val1, val2);
        val3 = _mm_add_pi32 (val3, val4);
        val1 = _mm_add_pi32 (val1, update_round);
        val3 = _mm_add_pi32 (val3, update_round);
        val1 = _mm_srai_pi32(val1, 4);
        val3 = _mm_srai_pi32(val3, 4);
        val1 = _mm_packs_pi32 (val1, val3);
        
        *(__m64*)out = _mm_add_pi16 (*(__m64*)out,val1);
        in1 +=4;
        in2 +=4;
        in3 +=4;
        in4 +=4;
        out +=4;
    }
        
    if (xstop != xend)
    {
        for ( i = xstop ; i < xend ; i++)
        {
               update.Filter( pic_data[ymid][i] , pic_data[yp][i], pic_data[yp+1][i], pic_data[yp][i],pic_data[yp+2][i]);
        }// i

        // middle bit
           for ( j=1 ; j < yl/2 - 2 ; ++j)
        {
            for ( i = xstop ; i < xend ; i++)
            {
                update.Filter( pic_data[ymid+j][i] , pic_data[yp+j][i], pic_data[yp+j+1][i], pic_data[yp+j-1][i],pic_data[yp+j+2][i]);
            }// i
        }// k
    
        for ( i = xstop ; i < xend ; i++)
        {
            update.Filter( pic_data[yend - 2][i] , pic_data[ymid-2][i], pic_data[ymid-1][i], pic_data[ymid-3][i],pic_data[ymid-1][i]);
            update.Filter( pic_data[yend - 1][i] , pic_data[ymid-1][i], pic_data[ymid-1][i], pic_data[ymid-2][i],pic_data[ymid-1][i]);
        }// i
    }


    // Horizontal sythesis
    
    const int xmid = xl/2;
    xstop = xmid %4 ? ((xmid>>2)<<2) + 1 : xmid -3;

    for (j = yp;  j < yend; ++j)
    {
        ValueType *line_data = &pic_data[j][xp];                 

        // First lifting stage acts on even samples i.e. the low pass ones
         predict.Filter( line_data[0] , line_data[xmid] , line_data[xmid] );
        for (i=1 ; i < xmid ; ++i)
        {
            predict.Filter( line_data[i] , line_data[xmid+i-1] , line_data[xmid+i] );
        }

        // Second lifting stage
           update.Filter( line_data[xmid] , line_data[0] , line_data[1] , line_data[0] , line_data[2] );

        for (i=1 ; i < xmid - 2; ++i)
        {
            update.Filter( line_data[xmid+i] , line_data[i] , line_data[i+1] , line_data[i-1] , line_data[i+2] );
        }// i 
        update.Filter( line_data[xl-2] , line_data[xmid-2] , line_data[xmid-1] , line_data[xmid-3] , line_data[xmid-1] );
        update.Filter( line_data[xl-1] , line_data[xmid-1] , line_data[xmid-1] , line_data[xmid-2] , line_data[xmid-1] );
        
        // Shift right by one bit to counter the shift in the analysis stage
        ShiftRowRight(line_data, xl, 1);

    }// j
    _mm_empty();
    Interleave_mmx( xp , yp , xl ,yl , pic_data );
}

void WaveletTransform::VHFilterDD13_5::Synth(const int xp ,
                                           const int yp , 
                                           const int xl ,
                                           const int yl , 
                                           PicArray& pic_data)
{
    int i,j,k;

    const int xend( xp+xl );
    const int yend( yp+yl );

    PredictStepFourTap< 5 , 9 , -1 > predict;
    __m64 pred_round = _mm_set_pi32 (1<<(5-1), 1<<(5-1));
    UpdateStepFourTap< 4 , 9 , -1> update;
    __m64 update_round = _mm_set_pi32 (1<<(4-1), 1<<(4-1));

    // Next, do the vertical synthesis
    int ymid = yp + yl/2;

    int xstop = xp + (xl>>2)<<2;
    // First lifting stage - odd samples
    // bottom edge
    ValueType *out = pic_data[ymid-1];
    ValueType *in1 = pic_data[yend-2];
    ValueType *in2 = pic_data[yend-1];
    ValueType *in3 = pic_data[yend-3];
    ValueType *in4 = pic_data[yend-1];

    __m64 tap1 = _mm_set_pi16 (9, 9, 9, 9);
    __m64 tap2 = _mm_set_pi16 (-1, -1, -1, -1);
    for ( i = xp ; i<xstop; i+=4)
    {
        __m64 val1, val2, val3, val4, tmp1, tmp2;

        val1 = _mm_add_pi16(*(__m64*)in1, *(__m64*)in2);
        tmp1 = _mm_mullo_pi16 (val1, tap1);
        tmp2 = _mm_mulhi_pi16 (val1, tap1);
        val3 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val1 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val2 = _mm_add_pi16(*(__m64*)in3, *(__m64*)in4);
        tmp1 = _mm_mullo_pi16 (val2, tap2);
        tmp2 = _mm_mulhi_pi16 (val2, tap2);
        val4 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val2 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val1 = _mm_add_pi32 (val1, val2);
        val3 = _mm_add_pi32 (val3, val4);
        val1 = _mm_add_pi32 (val1, pred_round);
        val3 = _mm_add_pi32 (val3, pred_round);
        val1 = _mm_srai_pi32(val1, 5);
        val3 = _mm_srai_pi32(val3, 5);
        val1 = _mm_packs_pi32 (val1, val3);

        *(__m64*)out = _mm_sub_pi16 (*(__m64*)out,val1);
        in1 +=4;
        in2 +=4;
        in3 +=4;
        in4 +=4;
        out +=4;
    }// i

    // middle bit
    for ( j = 2 ; j < yl/2 -1 ; ++j)
    {
        out = pic_data[yp+j];
        in1 = pic_data[ymid+j-1];
        in2 = pic_data[ymid+j];
        in3 = pic_data[ymid+j-2];
        in4 = pic_data[ymid+j+1];
        for ( i = xp ; i<xstop ; i+=4)
        {
            __m64 val1, val2, val3, val4, tmp1, tmp2;

            val1 = _mm_add_pi16(*(__m64*)in1, *(__m64*)in2);
            tmp1 = _mm_mullo_pi16 (val1, tap1);
            tmp2 = _mm_mulhi_pi16 (val1, tap1);
            val3 = _mm_unpackhi_pi16 (tmp1, tmp2);
            val1 = _mm_unpacklo_pi16 (tmp1, tmp2);

            val2 = _mm_add_pi16(*(__m64*)in3, *(__m64*)in4);
            tmp1 = _mm_mullo_pi16 (val2, tap2);
            tmp2 = _mm_mulhi_pi16 (val2, tap2);
            val4 = _mm_unpackhi_pi16 (tmp1, tmp2);
            val2 = _mm_unpacklo_pi16 (tmp1, tmp2);

            val1 = _mm_add_pi32 (val1, val2);
            val3 = _mm_add_pi32 (val3, val4);
            val1 = _mm_add_pi32 (val1, pred_round);
            val3 = _mm_add_pi32 (val3, pred_round);
            val1 = _mm_srai_pi32(val1, 5);
            val3 = _mm_srai_pi32(val3, 5);
            val1 = _mm_packs_pi32 (val1, val3);

            *(__m64*)out = _mm_sub_pi16 (*(__m64*)out,val1);
            in1 +=4;
            in2 +=4;
            in3 +=4;
            in4 +=4;
            out +=4;
        }// i
    }// j

    // top edge - j=xp
    out = pic_data[yp+1];
    in1 = pic_data[ymid];
    in2 = pic_data[ymid+1];
    in3 = pic_data[ymid+2];
    in4 = pic_data[ymid];
    for ( i = xp ; i<xstop ; i+=4)
    {
        __m64 val1, val2, val3, val4, tmp1, tmp2;

        val1 = _mm_add_pi16(*(__m64*)in1, *(__m64*)in2);
        tmp1 = _mm_mullo_pi16 (val1, tap1);
        tmp2 = _mm_mulhi_pi16 (val1, tap1);
        val3 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val1 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val2 = _mm_add_pi16(*(__m64*)in3, *(__m64*)in4);
        tmp1 = _mm_mullo_pi16 (val2, tap2);
        tmp2 = _mm_mulhi_pi16 (val2, tap2);
        val4 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val2 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val1 = _mm_add_pi32 (val1, val2);
        val3 = _mm_add_pi32 (val3, val4);
        val1 = _mm_add_pi32 (val1, pred_round);
        val3 = _mm_add_pi32 (val3, pred_round);
        val1 = _mm_srai_pi32(val1, 5);
        val3 = _mm_srai_pi32(val3, 5);
        val1 = _mm_packs_pi32 (val1, val3);

        *(__m64*)out = _mm_sub_pi16 (*(__m64*)out,val1);
        in1 +=4;
        in2 +=4;
        in3 +=4;
        in4 +=4;
        out +=4;
    }

    out = pic_data[yp];
    in1 = pic_data[ymid];
    in2 = pic_data[ymid];
    in3 = pic_data[ymid+1];
    in4 = pic_data[ymid];
    for ( i = xp ; i<xstop ; i+=4)
    {
        __m64 val1, val2, val3, val4, tmp1, tmp2;

        val1 = _mm_add_pi16(*(__m64*)in1, *(__m64*)in2);
        tmp1 = _mm_mullo_pi16 (val1, tap1);
        tmp2 = _mm_mulhi_pi16 (val1, tap1);
        val3 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val1 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val2 = _mm_add_pi16(*(__m64*)in3, *(__m64*)in4);
        tmp1 = _mm_mullo_pi16 (val2, tap2);
        tmp2 = _mm_mulhi_pi16 (val2, tap2);
        val4 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val2 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val1 = _mm_add_pi32 (val1, val2);
        val3 = _mm_add_pi32 (val3, val4);
        val1 = _mm_add_pi32 (val1, pred_round);
        val3 = _mm_add_pi32 (val3, pred_round);
        val1 = _mm_srai_pi32(val1, 5);
        val3 = _mm_srai_pi32(val3, 5);
        val1 = _mm_packs_pi32 (val1, val3);

        *(__m64*)out = _mm_sub_pi16 (*(__m64*)out,val1);
        in1 +=4;
        in2 +=4;
        in3 +=4;
        in4 +=4;
        out +=4;
    }

    // Mopup 
    if ( xstop != xend)
    {
        // Mopup bottom edge
        for ( i = xstop ; i<xend ; ++ i)
        {
            predict.Filter( pic_data[ymid-1][i] , pic_data[yend-2][i] , pic_data[yend-1][i] , pic_data[yend-3][i] , pic_data[yend-1][i] );
        }// i

        // Mopup middle bit
        for ( k = 2 ; k < yl/2 - 1 ; ++k)
        {
            for ( i = xstop ; i<xend ; ++ i)
            {
                   predict.Filter( pic_data[yp+k][i] , pic_data[ymid+k-1][i] , pic_data[ymid+k][i] , pic_data[ymid+k-2][i] , pic_data[ymid+k+1][i] );
            }// i
        }// k

        //Mopup top edge
        for ( i = xstop ; i<xend ; ++ i)
        {
            predict.Filter( pic_data[yp+1][i] , pic_data[ymid][i] , pic_data[ymid+1][i] , pic_data[ymid+2][i] , pic_data[ymid][i] );
            predict.Filter( pic_data[yp][i] , pic_data[ymid][i] , pic_data[ymid][i] , pic_data[ymid+1][i] , pic_data[ymid][i] );

        }// i

    }

    // Second lifting stage
    // top edge - j=xp
    out = pic_data[ymid];
    in1 = pic_data[yp];
    in2 = pic_data[yp+1];
    in3 = pic_data[yp];
    in4 = pic_data[yp+2];
    for ( i = xp ; i<xstop ; i+=4)
    {
        __m64 val1, val2, val3, val4, tmp1, tmp2;

        val1 = _mm_add_pi16(*(__m64*)in1, *(__m64*)in2);
        tmp1 = _mm_mullo_pi16 (val1, tap1);
        tmp2 = _mm_mulhi_pi16 (val1, tap1);
        val3 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val1 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val2 = _mm_add_pi16(*(__m64*)in3, *(__m64*)in4);
        tmp1 = _mm_mullo_pi16 (val2, tap2);
        tmp2 = _mm_mulhi_pi16 (val2, tap2);
        val4 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val2 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val1 = _mm_add_pi32 (val1, val2);
        val3 = _mm_add_pi32 (val3, val4);
        val1 = _mm_add_pi32 (val1, update_round);
        val3 = _mm_add_pi32 (val3, update_round);
        val1 = _mm_srai_pi32(val1, 4);
        val3 = _mm_srai_pi32(val3, 4);
        val1 = _mm_packs_pi32 (val1, val3);

        *(__m64*)out = _mm_add_pi16 (*(__m64*)out,val1);
        in1 +=4;
        in2 +=4;
        in3 +=4;
        in4 +=4;
        out +=4;
    }// i


    // middle bit
    for ( k = 1 ; k < yl/2  - 2  ; ++k)
    {
        out = pic_data[ymid+k];
        in1 = pic_data[k];
        in2 = pic_data[k+1];
        in3 = pic_data[k-1];
        in4 = pic_data[k+2];
        for ( i = xp ; i<xstop ; i+=4)
        {
            __m64 val1, val2, val3, val4, tmp1, tmp2;

            val1 = _mm_add_pi16(*(__m64*)in1, *(__m64*)in2);
            tmp1 = _mm_mullo_pi16 (val1, tap1);
            tmp2 = _mm_mulhi_pi16 (val1, tap1);
            val3 = _mm_unpackhi_pi16 (tmp1, tmp2);
            val1 = _mm_unpacklo_pi16 (tmp1, tmp2);

            val2 = _mm_add_pi16(*(__m64*)in3, *(__m64*)in4);
            tmp1 = _mm_mullo_pi16 (val2, tap2);
            tmp2 = _mm_mulhi_pi16 (val2, tap2);
            val4 = _mm_unpackhi_pi16 (tmp1, tmp2);
            val2 = _mm_unpacklo_pi16 (tmp1, tmp2);

            val1 = _mm_add_pi32 (val1, val2);
            val3 = _mm_add_pi32 (val3, val4);
            val1 = _mm_add_pi32 (val1, update_round);
            val3 = _mm_add_pi32 (val3, update_round);
            val1 = _mm_srai_pi32(val1, 4);
            val3 = _mm_srai_pi32(val3, 4);
            val1 = _mm_packs_pi32 (val1, val3);

            *(__m64*)out = _mm_add_pi16 (*(__m64*)out,val1);
            in1 +=4;
            in2 +=4;
            in3 +=4;
            in4 +=4;
            out +=4;
        }// i
    }// k

    // bottom edge
    out = pic_data[yend-2];
    in1 = pic_data[ymid-2];
    in2 = pic_data[ymid-1];
    in3 = pic_data[ymid-3];
    in4 = pic_data[ymid-1];
    for ( i = xp ; i<xstop ; i+=4)
    {
        __m64 val1, val2, val3, val4, tmp1, tmp2;

        val1 = _mm_add_pi16(*(__m64*)in1, *(__m64*)in2);
        tmp1 = _mm_mullo_pi16 (val1, tap1);
        tmp2 = _mm_mulhi_pi16 (val1, tap1);
        val3 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val1 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val2 = _mm_add_pi16(*(__m64*)in3, *(__m64*)in4);
        tmp1 = _mm_mullo_pi16 (val2, tap2);
        tmp2 = _mm_mulhi_pi16 (val2, tap2);
        val4 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val2 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val1 = _mm_add_pi32 (val1, val2);
        val3 = _mm_add_pi32 (val3, val4);
        val1 = _mm_add_pi32 (val1, update_round);
        val3 = _mm_add_pi32 (val3, update_round);
        val1 = _mm_srai_pi32(val1, 4);
        val3 = _mm_srai_pi32(val3, 4);
        val1 = _mm_packs_pi32 (val1, val3);

        *(__m64*)out = _mm_add_pi16 (*(__m64*)out,val1);
        in1 +=4;
        in2 +=4;
        in3 +=4;
        in4 +=4;
        out +=4;
    }// i

    out = pic_data[yend-1];
    in1 = pic_data[ymid-1];
    in2 = pic_data[ymid-1];
    in3 = pic_data[ymid-2];
    in4 = pic_data[ymid-1];
    for ( i = xp ; i<xstop ; i+=4)
    {
        __m64 val1, val2, val3, val4, tmp1, tmp2;

        val1 = _mm_add_pi16(*(__m64*)in1, *(__m64*)in2);
        tmp1 = _mm_mullo_pi16 (val1, tap1);
        tmp2 = _mm_mulhi_pi16 (val1, tap1);
        val3 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val1 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val2 = _mm_add_pi16(*(__m64*)in3, *(__m64*)in4);
        tmp1 = _mm_mullo_pi16 (val2, tap2);
        tmp2 = _mm_mulhi_pi16 (val2, tap2);
        val4 = _mm_unpackhi_pi16 (tmp1, tmp2);
        val2 = _mm_unpacklo_pi16 (tmp1, tmp2);

        val1 = _mm_add_pi32 (val1, val2);
        val3 = _mm_add_pi32 (val3, val4);
        val1 = _mm_add_pi32 (val1, update_round);
        val3 = _mm_add_pi32 (val3, update_round);
        val1 = _mm_srai_pi32(val1, 4);
        val3 = _mm_srai_pi32(val3, 4);
        val1 = _mm_packs_pi32 (val1, val3);

        *(__m64*)out = _mm_add_pi16 (*(__m64*)out,val1);
        in1 +=4;
        in2 +=4;
        in3 +=4;
        in4 +=4;
        out +=4;
    }// i
    

    // Mopup 
    if ( xstop != xend)
    {
        // bottom edge
        for ( i = xstop ; i<xend ; ++ i)
        {
            update.Filter( pic_data[yend-1][i] , pic_data[ymid-1][i] , pic_data[ymid-1][i] , pic_data[ymid-2][i] , pic_data[ymid-1][i] );
            update.Filter( pic_data[yend-2][i] , pic_data[ymid-2][i] , pic_data[ymid-1][i] , pic_data[ymid-3][i] , pic_data[ymid-1][i] );

        }// i

        // middle bit
        for ( k = 1 ; k < yl/2  - 2  ; ++k)
        {
            for ( i = xstop ; i<xend ; ++ i)
            {
                update.Filter( pic_data[ymid+k][i] , pic_data[k][i] , pic_data[k+1][i] , pic_data[k-1][i] , pic_data[k+2][i] );
            }// i
        }// j

        // top edge - j=xp
        for ( i = xstop ; i<xend ; ++ i)
        {
            update.Filter( pic_data[ymid][i] , pic_data[yp][i] , pic_data[yp+1][i] , pic_data[yp][i] , pic_data[yp+2][i] ); 
        }// i
    }

    // Next do the horizontal synthesis

    ValueType* line_data;
    int xmid = xl/2;

    for (j = yp;  j < yend ; ++j)
    {
        line_data = &pic_data[j][xp];                 

        // First lifting stage

        predict.Filter( line_data[0] , line_data[xmid] , line_data[xmid] , line_data[xmid+1] , line_data[xmid] );
        predict.Filter( line_data[1] , line_data[xmid] , line_data[xmid+1] , line_data[xmid+2] , line_data[xmid] );

        for (k=2 ; k < xmid-1 ; ++k)
        {
            predict.Filter( line_data[k] , line_data[xmid+k-1] , line_data[xmid+k] , line_data[xmid+k-2] , line_data[xmid+k+1] );

        }// i 
        predict.Filter( line_data[xmid-1] , line_data[xl-2] , line_data[xl-1] , line_data[xl-3] , line_data[xl-1] );

         //second lifting stage 

        update.Filter( line_data[xmid] , line_data[0] , line_data[1] , line_data[0] , line_data[2] ); 
        for (k=1 ; k<xmid-2 ; ++k)
        {
            update.Filter( line_data[xmid+k] , line_data[k] , line_data[k+1] , line_data[k-1] , line_data[k+2] );
        }// i 
        update.Filter( line_data[xl-2] , line_data[xmid-2] , line_data[xmid-1] , line_data[xmid-3] , line_data[xmid-1] );
        update.Filter( line_data[xl-1] , line_data[xmid-1] , line_data[xmid-1] , line_data[xmid-2] , line_data[xmid-1] );

        // Shift right by one bit to counter the shift in the analysis stage
        ShiftRowRight(line_data, xl, 1);

    }// j

    _mm_empty();
    // Interleave subbands 
    Interleave_mmx( xp , yp , xl , yl , pic_data );  
}

#if 0
//Opts - Attempt1
void WaveletTransform::VHFilterLEGALL5_3::Synth(const int xp ,
                                          const int yp , 
                                          const int xl , 
                                          const int yl , 
                                          PicArray& pic_data)
{
    int i,j,k;

    const int xend( xp+xl );
    const int yend( yp+yl );

    const PredictStepShift< 2 > predict;
    const UpdateStepShift< 1 > update;

    ValueType* line_data;

    // Firstly reorder to interleave subbands, so that subsequent calculations 
    // can be in-place
    Interleave_mmx( xp , yp , xl , yl , pic_data );

    // Next, do the vertical synthesis
    // First lifting stage
    int xstop = (xend>>2)<<2;
    
    // Begin the top edge
    ValueType *row1, *row2, *row3, *row4;

    row1 = &pic_data[yp][xp];
    row2 = &pic_data[yp+1][xp];
    for ( i = xp ; i < xstop ; i+=4)
    {
        __m64 tmp = _mm_add_pi16 (*(__m64 *)row2, *(__m64 *)row2);
        tmp = _mm_srai_pi16(tmp, 2);
        *(__m64 *)row1 = _mm_sub_pi16 (*(__m64*)row1, tmp);

        row1 += 4;
        row2 += 4;
    }
    // Mopup
    for ( i = xstop ; i < xend ; ++i)
    {
        predict.Filter( pic_data[yp][i] , pic_data[yp+1][i] , pic_data[yp+1][i] );
    }// i


    // Next, do the middle bit
    for ( k = yp+2 ; k < yend-2 ; k+=2)
    {
        ValueType *row1 = &pic_data[k-2][xp];
        ValueType *row2 = &pic_data[k-1][xp];
        ValueType *row3 = &pic_data[k][xp];
        ValueType *row4 = &pic_data[k+1][xp];

        for ( i = xp ; i < xstop ; i+=4)
        {
            __m64 tmp = _mm_add_pi16 (*(__m64 *)row4, *(__m64 *)row2);
            tmp = _mm_srai_pi16(tmp, 2);
            *(__m64 *)row3 = _mm_sub_pi16 (*(__m64*)row3, tmp);

            tmp = _mm_add_pi16 (*(__m64 *)row1, *(__m64 *)row3);
            tmp = _mm_srai_pi16(tmp, 1);
            *(__m64 *)row2 = _mm_add_pi16 (*(__m64*)row2, tmp);
            row1 += 4;
            row2 += 4;
            row3 += 4;
            row4 += 4;
        }// i

        //Mopup
        for ( i = xstop ; i < xend ; ++i)
        {
            predict.Filter( pic_data[k][i] , pic_data[k+1][i] , pic_data[k-1][i] );
            update.Filter( pic_data[k-1][i] , pic_data[k-2][i] , pic_data[k][i] );
        }// i
    }// j
    
    // Finally with the bottom edge
    row1 = &pic_data[yend-4][xp];
    row2 = &pic_data[yend-3][xp];
    row3 = &pic_data[yend-2][xp];
    row4 = &pic_data[yend-1][xp];

    for ( i = xp ; i < xstop ; i+=4)
    {
        __m64 tmp = _mm_add_pi16 (*(__m64 *)row2, *(__m64 *)row4);
        tmp = _mm_srai_pi16(tmp, 2);
        *(__m64 *)row3 = _mm_sub_pi16 (*(__m64*)row3, tmp);

        tmp = _mm_add_pi16 (*(__m64 *)row3, *(__m64 *)row1);
        tmp = _mm_srai_pi16(tmp, 1);
        *(__m64 *)row2 = _mm_add_pi16 (*(__m64*)row2, tmp);

        tmp = _mm_add_pi16 (*(__m64 *)row3, *(__m64 *)row3);
        tmp = _mm_srai_pi16(tmp, 1);
        *(__m64 *)row4 = _mm_add_pi16 (*(__m64*)row4, tmp);

        row1 += 4;
        row2 += 4;
        row3 += 4;
        row4 += 4;
    }// i
    // mopup
    for ( i = xstop ; i < xend ; ++i)
    {
        predict.Filter( pic_data[yend-2][i] , pic_data[yend-3][i] , pic_data[yend-1][i] );
        update.Filter( pic_data[yend-3][i] , pic_data[yend-2][i] , pic_data[yend-4][i] );
        update.Filter( pic_data[yend-1][i] , pic_data[yend-2][i] , pic_data[yend-2][i] );
    }// i


    // Next do the horizontal synthesis
    for (j = yp;  j < yend ; ++j)
    {
        // First lifting stage 
        line_data = &pic_data[j][xp];

        predict.Filter( line_data[0] , line_data[1] , line_data[1] );

        for ( k = 2;  k < xl -2; k+=2)
        { 
            predict.Filter( line_data[k] , line_data[k+1] , line_data[k-1] );
            update.Filter( line_data[k-1] , line_data[k-2] , line_data[k] );
        }// i
        
        predict.Filter( line_data[xl-2] , line_data[xl-3] , line_data[xl-1] ); 
        update.Filter( line_data[xl-3] , line_data[xl-2] , line_data[xl-4] );
        update.Filter( line_data[xl-1] , line_data[xl-2] , line_data[xl-2] );

    }
    _mm_empty();
}
#endif

#if 0
//Opts Attempt 2
void WaveletTransform::VHFilterLEGALL5_3::Synth(const int xp ,
                                          const int yp , 
                                          const int xl , 
                                          const int yl , 
                                          PicArray& pic_data)
{
    int i,j,k;
    const int yend( yp+yl );

    const int xl2 (xl>>1);
    const int ymid (yp + (yl>>1));
    const PredictStepShift< 2 > predict;
    const UpdateStepShift< 1 > update;

    ValueType* line_data;


    // Next, do the vertical synthesis
    // First lifting stage

    int xstop = (xl>>2)<<2;

    ValueType *row1, *row2, *row3, *row4;
    // First do the top edge
    row1 = &pic_data[yp][xp];
    row2 = &pic_data[ymid][xp];
    for ( i = 0 ; i < xstop ; i+=4)
    {
        __m64 tmp = _mm_add_pi16 (*(__m64 *)row2, *(__m64 *)row2);
        tmp = _mm_srai_pi16(tmp, 2);
        *(__m64 *)row1 = _mm_sub_pi16 (*(__m64*)row1, tmp);
        row1+=4;
        row2+=4;
    }// i

    //mopup
    for ( i = xstop ; i < xl ; ++i)
    {
        predict.Filter( *row1 , *row2 , *row2);
        ++row1;
        ++row2;
    }// i

    // Next, do the middle bit
    for ( k = 1 ; k < ymid-1 ; ++k)
    {
        row1 = &pic_data[k-1][xp];
        row2 = &pic_data[k][xp];
        row3 = &pic_data[ymid+k-1][xp];
        row4 = &pic_data[ymid+k][xp];

        for ( i = 0 ; i < xstop ; i+=4)
        {
            __m64 tmp = _mm_add_pi16 (*(__m64 *)row3, *(__m64 *)row4);
            tmp = _mm_srai_pi16(tmp, 2);
            *(__m64 *)row2 = _mm_sub_pi16 (*(__m64*)row2, tmp);

            tmp = _mm_add_pi16 (*(__m64 *)row2, *(__m64 *)row1);
            tmp = _mm_srai_pi16(tmp, 1);
            *(__m64 *)row3 = _mm_add_pi16 (*(__m64*)row3, tmp);
            
            row1 += 4;
            row2 += 4;
            row3 += 4;
            row4 += 4;
        }// i

        for ( i = xstop ; i < xl ; ++i)
        {
            predict.Filter( *row2 , *row4 , *row3 );
            update.Filter( *row3 , *row2 , *row1 );
            ++row1;
            ++row2;
            ++row3;
            ++row4;
        }// i
    }// j


    // Finally with the bottom edge
    row1 = &pic_data[ymid-2][xp];
    row2 = &pic_data[ymid-1][xp];
    row3 = &pic_data[yend-2][xp];
    row4 = &pic_data[yend-1][xp];
    for ( i = xp ; i< xstop ; i+=4)
    {
        __m64 tmp = _mm_add_pi16 (*(__m64 *)row3, *(__m64 *)row4);
        tmp = _mm_srai_pi16(tmp, 2);
        *(__m64 *)row2 = _mm_sub_pi16 (*(__m64*)row2, tmp);

        tmp = _mm_add_pi16 (*(__m64 *)row2, *(__m64 *)row1);
        tmp = _mm_srai_pi16(tmp, 1);
        *(__m64 *)row3 = _mm_add_pi16 (*(__m64*)row3, tmp);
           
        tmp = _mm_add_pi16 (*(__m64 *)row2, *(__m64 *)row2);
        tmp = _mm_srai_pi16(tmp, 1);
        *(__m64 *)row4 = _mm_add_pi16 (*(__m64*)row4, tmp);
           
        row1 += 4;
        row2 += 4;
        row3 += 4;
        row4 += 4;
    }// i
    // mopup
    for ( i = xstop ; i< xl ; ++i)
    {
        predict.Filter( *row2 , *row3 , *row4 );
        update.Filter( *row3 , *row1 , *row2 );
        update.Filter( *row4 , *row2 , *row2 );
        ++row1;
        ++row2;
        ++row3;
        ++row4;
    }// i

    // Next do the horizontal synthesis
    xstop = (((xl2 - 2)>>2)<<2) + 1;
    //xstop = 1;
    for (j = yp;  j < yend ; ++j)
    {
        // First lifting stage 
        line_data = &pic_data[j][xp];
        predict.Filter( line_data[0] , line_data[xl2] , line_data[xl2] );

        for ( k = 1;  k < xstop; k+=4)
        { 
            //predict.Filter( line_data[k] , line_data[xl2+k] , line_data[xl2+k-1] );
            __m64 m1 = _mm_add_pi16 (*(__m64 *)(line_data+xl2+k), *(__m64 *)(line_data+xl2+k-1));
            m1 = _mm_srai_pi16 (m1, 2);
            *(__m64 *)(line_data+k) = _mm_sub_pi16 (*(__m64 *)(line_data+k), m1);

            //update.Filter( line_data[xl2+k-1] , line_data[k] , line_data[k-1] );
            m1 = _mm_add_pi16 (*(__m64 *)(line_data+k), *(__m64 *)(line_data+k-1));
            m1 = _mm_srai_pi16(m1, 1);
            *(__m64 *)(line_data+xl2+k-1) = _mm_add_pi16 (*(__m64*)(line_data+xl2+k-1), m1);
            
            row1 += 4;
            row2 += 4;
            row3 += 4;
            row4 += 4;
        }// i


        for ( k = xstop;  k < xl2-1; ++k)
        { 
            predict.Filter( line_data[k] , line_data[xl2+k] , line_data[xl2+k-1] );
            update.Filter( line_data[xl2+k-1] , line_data[k] , line_data[k-1] );
        }// i

        predict.Filter( line_data[xl2-1] , line_data[xl-2] , line_data[xl-1] ); 
        update.Filter( line_data[xl-2] , line_data[xl2-2] , line_data[xl2-1] );
        update.Filter( line_data[xl-1] , line_data[xl2-1] , line_data[xl2-1] );
    }
    _mm_empty();
    
    // Finally interleave subbands
    Interleave_mmx( xp , yp , xl , yl , pic_data );
}
#endif

//Attempt 3

inline void WaveletTransform::VHFilterLEGALL5_3::HorizSynth (int xp, int xl, int ystart, int yend, PicArray &pic_data)
{
    static const PredictStepShift< 2 > predict;
    static const UpdateStepShift< 1 > update;
    int j, k;
    // Next do the horizontal synthesis
    for (j = ystart;  j <= yend ; ++j)
    {
        // First lifting stage 
        ValueType *line_data = &pic_data[j][xp];

        predict.Filter( line_data[0] , line_data[1] , line_data[1] );

        for ( k = 2;  k < xl -2; k+=2)
        { 
            predict.Filter( line_data[k] , line_data[k+1] , line_data[k-1] );
            update.Filter( line_data[k-1] , line_data[k-2] , line_data[k] );
        }// i
        
        predict.Filter( line_data[xl-2] , line_data[xl-3] , line_data[xl-1] ); 
        update.Filter( line_data[xl-3] , line_data[xl-2] , line_data[xl-4] );
        update.Filter( line_data[xl-1] , line_data[xl-2] , line_data[xl-2] );
        // Shift right by one bit to counter the shift in the analysis stage
        ShiftRowRight(line_data, xl, 1);
    }
}

void WaveletTransform::VHFilterLEGALL5_3::Synth(const int xp ,
                                          const int yp , 
                                          const int xl , 
                                          const int yl , 
                                          PicArray &pic_data)
{
    int i, k;

    const int xend( xp+xl );
    const int yend( yp+yl );

    const PredictStepShift< 2 > predict;
    __m64 pred_round = _mm_set_pi16 (1<<(2-1), 1<<(2-1), 1<<(2-1), 1<<(2-1));
    const UpdateStepShift< 1 > update;
    __m64 update_round = _mm_set_pi16 (1, 1, 1, 1);

    int horiz_start = 0;
    int horiz_end = 0;

    // Firstly reorder to interleave subbands, so that subsequent calculations 
    // can be in-place
    Interleave_mmx( xp , yp , xl , yl , pic_data );

    // Next, do the vertical synthesis
    // First lifting stage
    int xstop = (xend>>2)<<2;
    
    // Begin the top edge
    ValueType *row1, *row2, *row3, *row4;

    row1 = &pic_data[yp][xp];
    row2 = &pic_data[yp+1][xp];
    for ( i = xp ; i < xstop ; i+=4)
    {
        __m64 tmp = _mm_add_pi16 (*(__m64 *)row2, *(__m64 *)row2);
        tmp = _mm_add_pi16 (tmp, pred_round);
        tmp = _mm_srai_pi16(tmp, 2);
        *(__m64 *)row1 = _mm_sub_pi16 (*(__m64*)row1, tmp);

        row1 += 4;
        row2 += 4;
    }
    // Mopup
    for ( i = xstop ; i < xend ; ++i)
    {
        predict.Filter( *row1, *row2, *row2 );
        ++row1;
        ++row2;
    }// i


    // Next, do the middle bit
    for ( k = yp+2 ; k < yend-2 ; k+=2)
    {
        ValueType *row1 = &pic_data[k-2][xp];
        ValueType *row2 = &pic_data[k-1][xp];
        ValueType *row3 = &pic_data[k][xp];
        ValueType *row4 = &pic_data[k+1][xp];

        for ( i = xp ; i < xstop ; i+=4)
        {
            __m64 tmp = _mm_add_pi16 (*(__m64 *)row4, *(__m64 *)row2);
            tmp = _mm_add_pi16 (tmp, pred_round);
            tmp = _mm_srai_pi16(tmp, 2);
            *(__m64 *)row3 = _mm_sub_pi16 (*(__m64*)row3, tmp);

            tmp = _mm_add_pi16 (*(__m64 *)row1, *(__m64 *)row3);
            tmp = _mm_add_pi16 (tmp, update_round);
            tmp = _mm_srai_pi16(tmp, 1);
            *(__m64 *)row2 = _mm_add_pi16 (*(__m64*)row2, tmp);
            row1 += 4;
            row2 += 4;
            row3 += 4;
            row4 += 4;
        }// i

        //Mopup
        for ( i = xstop ; i < xend ; ++i)
        {
            predict.Filter( *row3, *row2, *row4 );
            update.Filter( *row2, *row1, *row3 );
            ++row1;
            ++row2;
            ++row3;
            ++row4;
        }// i
        horiz_end = k - 2;
        // Do the horizontal synthesis
        HorizSynth (xp, xl, horiz_start, horiz_end, pic_data);
        horiz_start = horiz_end + 1;
    }// j
    
    // Finally with the bottom edge
    row1 = &pic_data[yend-4][xp];
    row2 = &pic_data[yend-3][xp];
    row3 = &pic_data[yend-2][xp];
    row4 = &pic_data[yend-1][xp];

    for ( i = xp ; i < xstop ; i+=4)
    {
        __m64 tmp = _mm_add_pi16 (*(__m64 *)row2, *(__m64 *)row4);
        tmp = _mm_add_pi16 (tmp, pred_round);
        tmp = _mm_srai_pi16(tmp, 2);
        *(__m64 *)row3 = _mm_sub_pi16 (*(__m64*)row3, tmp);

        tmp = _mm_add_pi16 (*(__m64 *)row3, *(__m64 *)row1);
           tmp = _mm_add_pi16 (tmp, update_round);
        tmp = _mm_srai_pi16(tmp, 1);
        *(__m64 *)row2 = _mm_add_pi16 (*(__m64*)row2, tmp);

        tmp = _mm_add_pi16 (*(__m64 *)row3, *(__m64 *)row3);
        tmp = _mm_add_pi16 (tmp, update_round);
        tmp = _mm_srai_pi16(tmp, 1);
        *(__m64 *)row4 = _mm_add_pi16 (*(__m64*)row4, tmp);

        row1 += 4;
        row2 += 4;
        row3 += 4;
        row4 += 4;
    }// i
    // mopup
    for ( i = xstop ; i < xend ; ++i)
    {
        predict.Filter( *row3, *row2, *row4 );
        update.Filter( *row2, *row1, *row3 );
        update.Filter( *row4, *row3, *row3 );
        ++row1;
        ++row2;
        ++row3;
        ++row4;
    }// i

    _mm_empty();
    // Last lines of horizontal synthesis
    HorizSynth (xp, xl, horiz_start, yend-1, pic_data);
}


void DeInterleave_mmx( const int xp , 
                 const int yp , 
                 const int xl , 
                 const int yl , 
                 PicArray &pic_data)
{
    const int xl2( xl>>1);
    const int yl2( yl>>1);
    const int yend( yp + yl );

    if (pic_data.LengthX() > t_temp_data.LengthX() ||
        pic_data.LengthY() > t_temp_data.LengthY())
    {
        t_temp_data.Resize(pic_data.LengthY(), pic_data.LengthX());
    }

    // Make a temporary copy of the subband
    for (int j = yp; j<yend ; j++ )
        memcpy( t_temp_data[j-yp] , pic_data[j]+xp , xl * sizeof( ValueType ) );

    int stopx = (xl2>>2)<<2;
    
    for (int j = yp, s=0; j<(yp+yl2) ; j++, s+=2)
    {
        ValueType *tmp1 = &t_temp_data[s][0];
        ValueType *out1 = &pic_data[j][xp];
        ValueType *out2 = &pic_data[j][xl2];
        int r = xp;
        for (int i = 0; i<xp+stopx ; i+=4 , r+=8)
        {
            __m64 m1 = _mm_unpacklo_pi16 (*(__m64 *)tmp1, *(__m64 *)(tmp1+4));
            __m64 m2 = _mm_unpackhi_pi16 (*(__m64 *)tmp1, *(__m64 *)(tmp1+4));
            *(__m64 *)out1 = _mm_unpacklo_pi16 (m1, m2);
            *(__m64 *)out2 = _mm_unpackhi_pi16 (m1, m2);
            out1 += 4;
            out2 += 4;
            tmp1+=8;
        }
        //mopup
        for (int i = xp+stopx; i < xp+xl2; ++i, r+=2)
        {
            pic_data[j][i] = t_temp_data[s][r];
            pic_data[j][i+xl2] = t_temp_data[s][r+1];
        }
    }// j 

    for (int j = yl2, s=1; j< yend ; j++, s+=2)
    {
        ValueType *tmp1 = &t_temp_data[s][0];
        ValueType *out1 = &pic_data[j][xp];
        ValueType *out2 = &pic_data[j][xl2];
        int r = xp;
        for (int i = 0; i<xp+stopx ; i+=4 , r+=8)
        {
            __m64 m1 = _mm_unpacklo_pi16 (*(__m64 *)tmp1, *(__m64 *)(tmp1+4));
            __m64 m2 = _mm_unpackhi_pi16 (*(__m64 *)tmp1, *(__m64 *)(tmp1+4));
            *(__m64 *)out1 = _mm_unpacklo_pi16 (m1, m2);
            *(__m64 *)out2 = _mm_unpackhi_pi16 (m1, m2);
            out1 += 4;
            out2 += 4;
            tmp1+=8;
        }
        //mopup
        for (int i = xp+stopx; i < xp+xl2; ++i, r+=2)
        {
            pic_data[j][i] = t_temp_data[s][r];
            pic_data[j][i+xl2] = t_temp_data[s][r+1];
        }
    }// j 
    _mm_empty();
}

void WaveletTransform::VHFilterLEGALL5_3::Split(const int xp , 
                                          const int yp , 
                                          const int xl , 
                                          const int yl , 
                                          PicArray& pic_data)
{
    //version based on integer-like types
    //using edge-extension rather than reflection

    const int xend=xp+xl;
    const int yend=yp+yl;
    const int xl2 = xl>>1;
    const int yl2 = yl>>1;

    ValueType* line_data; 

    // Positional variables
    int i,j,k; 
  
    // Objects to do lifting stages 
    // (in revese order and type from synthesis)
    const PredictStepShift< 1 > predict;
    __m64 pred_round = _mm_set_pi16 (1, 1, 1, 1);
    const UpdateStepShift< 2 > update;
    __m64 update_round = _mm_set_pi16 (1<<(2-1), 1<<(2-1), 1<<(2-1), 1<<(2-1));

    // Lastly, have to reorder so that subbands are no longer interleaved
    DeInterleave_mmx( xp , yp , xl , yl , pic_data );
     //first do horizontal 

    for (j = yp;  j < yend; ++j)
    {
        // First lifting stage
        line_data = &pic_data[j][xp];
        // Shift left by one bit to give us more accuracy
        ShiftRowLeft(line_data, xl, 1);

        predict.Filter( line_data[xp+xl2] , line_data[1] , line_data[0] );
        update.Filter( line_data[0] , line_data[xp+xl2] , line_data[xp+xl2] );

        for (k = 1; k < xp+xl2-1; k+=1)
        {
            predict.Filter( line_data[xp+xl2+k] , line_data[k+1] , line_data[k] );
            update.Filter(  line_data[k] , line_data[xp+xl2+k-1] , line_data[xp+xl2+k] );
        }// i
        
        predict.Filter( line_data[xl-1] , line_data[xp+xl2-1] , line_data[xp+xl2-1] );
        update.Filter( line_data[xp+xl2-1] , line_data[xl-2] , line_data[xl-1] );

    }// j

    // next do vertical

    // First lifting stage

    // top edge - j=xp
    int stopX = (xl>>2)<<2;
    ValueType *in_val = &pic_data[yp+yl2][xp];
    ValueType *val1 = &pic_data[1][xp];
    ValueType *val2 = &pic_data[0][xp];
    for ( i = xp ; i<(xp+stopX); i+=4)
    {
        //predict.Filter( pic_data[yp+yl2][i] , pic_data[1][i] , pic_data[0][i] );
        __m64 m1 = _mm_add_pi16 (*(__m64 *)val1, *(__m64 *)val2);
        m1 = _mm_add_pi16 (m1, pred_round);
        m1 = _mm_srai_pi16(m1, 1);
        *(__m64 *)in_val = _mm_sub_pi16 (*(__m64 *)in_val, m1);

        //update.Filter( pic_data[0][i] , pic_data[yp+yl2][i] , pic_data[yp+yl2][i] );
        m1 = _mm_add_pi16 (*(__m64 *)in_val, *(__m64 *)in_val);
        m1 = _mm_add_pi16 (m1, update_round);
        m1 = _mm_srai_pi16(m1, 2);
        *(__m64 *)val2 = _mm_add_pi16 (*(__m64 *)val2, m1);
        in_val += 4;
        val1 += 4;
        val2 += 4;
    }// i
    // mopup
    for ( i = xp+stopX ; i<xend ; ++ i)
    {
        predict.Filter( pic_data[yp+yl2][i] , pic_data[1][i] , pic_data[0][i] );
        update.Filter( pic_data[0][i] , pic_data[yp+yl2][i] , pic_data[yp+yl2][i] );
    }// i

    // middle bit
    for (k = 1 ; k<yp+yl2-1 ; k+=1)
    {
        ValueType *in_val = &pic_data[yp+yl2+k][xp];
        ValueType *in_val2 = &pic_data[yp+yl2+k-1][xp];
        ValueType *val1 = &pic_data[k+1][xp];
        ValueType *val2 = &pic_data[k][xp];
        for ( i = xp ; i<xp+stopX ; i+=4)
        {
            //predict.Filter( pic_data[yp+yl2+k][i] , pic_data[k+1][i] , pic_data[k][i] );
            __m64 m1 = _mm_add_pi16 (*(__m64 *)val1, *(__m64 *)val2);
            m1 = _mm_add_pi16 (m1, pred_round);
            m1 = _mm_srai_pi16(m1, 1);
            *(__m64 *)in_val = _mm_sub_pi16 (*(__m64 *)in_val, m1);
            
            //update.Filter( pic_data[k][i] , pic_data[yp+yl2+k-1][i] , pic_data[yp+yl2+k][i] );
            m1 = _mm_add_pi16 (*(__m64 *)in_val, *(__m64 *)in_val2);
            m1 = _mm_add_pi16 (m1, update_round);
            m1 = _mm_srai_pi16(m1, 2);
            *(__m64 *)val2 = _mm_add_pi16 (*(__m64 *)val2, m1);

            in_val += 4;
            in_val2 += 4;
            val1 += 4;
            val2 += 4;
        }// i

        //mopup
        for ( i = xp+stopX ; i<xend ; ++ i)
        {
            predict.Filter( pic_data[yp+yl2+k][i] , pic_data[k+1][i] , pic_data[k][i] );
            update.Filter( pic_data[k][i] , pic_data[yp+yl2+k-1][i] , pic_data[yp+yl2+k][i] );
        }// i
    }// j

    in_val = &pic_data[yend-1][xp];
    val2 = &pic_data[yp+yl2-1][xp];
    ValueType *in_val2 = &pic_data[yend-2][xp];
    // bottom edge
    for ( i = xp ; i<xp+stopX ; i+=4)
    {
        //predict.Filter( pic_data[yend-1][i] , pic_data[yp+yl2-1][i] , pic_data[yp+yl2-1][i] );
        __m64 m1 = _mm_add_pi16 (*(__m64 *)val2, *(__m64 *)val2);
        m1 = _mm_add_pi16 (m1, pred_round);
        m1 = _mm_srai_pi16(m1, 1);
        *(__m64 *)in_val = _mm_sub_pi16 (*(__m64 *)in_val, m1);
       
       //update.Filter( pic_data[yp+yl2-1][i] , pic_data[yend-2][i] , pic_data[yend-1][i] );
        m1 = _mm_add_pi16 (*(__m64 *)in_val2, *(__m64 *)in_val);
        m1 = _mm_add_pi16 (m1, update_round);
        m1 = _mm_srai_pi16(m1, 2);
        *(__m64 *)val2 = _mm_add_pi16 (*(__m64 *)val2, m1);

        in_val += 4;
        in_val2 += 4;
        val2 += 4;
    }// i
    // mopup
    for ( i = xp+stopX ; i<xend ; ++ i)
    {
        predict.Filter( pic_data[yend-1][i] , pic_data[yp+yl2-1][i] , pic_data[yp+yl2-1][i] );
        update.Filter( pic_data[yp+yl2-1][i] , pic_data[yend-2][i] , pic_data[yend-1][i] );
    }// i
    _mm_empty();
}
#endif