/*
    Project  : H.264 encoder test    
    Module   : Quantization

    Author   : Marko 'Fador' Viitanen

    Date     : 28/6/2009
    Modified : 28/6/2009
*/
#include "common.h"
#include "tables.h"


/*
//Inverse
double E_i[4*4]= {
    0.25000,   0.31623,   0.25000,   0.31623,
    0.31623,   0.40000,   0.31623,   0.40000,
    0.25000,   0.31623,   0.25000,   0.31623,
    0.31623,   0.40000,   0.31623,   0.40000
};

//E=[a^2,(a*b)/2,a^2,(a*b)/2;(1*b)/2,(b^2)/4,(a*b)/2,(b^2)/4;a^2,(a*b)/2,a^2,(a*b)/2;(a*b)/2,(b^2)/4,(a*b)/2,(b^2)/4]

//Forward
double E_f[4*4]={
   0.25000,   0.15811,   0.25000,   0.15811,
   0.15811,   0.10000,   0.15811,   0.10000,
   0.25000,   0.15811,   0.25000,   0.15811,
   0.15811,   0.10000,   0.15811,   0.10000

};
*/

void init_QPstep()
{
    static const double Qsteps[6] = { 0.625, 0.6875, 0.8125, 0.875, 1.0, 1.125 };
    uint8 i;

    //QPstep doubles every 6th QP
    for(i=0;i<52;i++)
    {
        Qstep[i]=Qsteps[i%6]*(1<<(i/6));
    }
}

void init_MF()
{
    uint8 QP;

    uint16 PF1[6]={13107,11916,10082,9362,8192,7282};
    uint16 PF2[6]={5243,4660,4194,3647,3355,2893};
    uint16 PF3[6]={8066,7490,6554,5825,5243,4559};

    for(QP=0;QP<52;QP++)
    {
        MF[QP][0]=PF1[QP%6];
        MF[QP][1]=PF2[QP%6];
        MF[QP][2]=PF3[QP%6];        
    }    
}

void init_V()
{
    uint32 QP;

    uint32 V1[6]={10,11,13,14,16,18};
    uint32 V2[6]={16,18,20,23,25,29};
    uint32 V3[6]={13,14,16,18,20,23};

    for(QP=0;QP<52;QP++)
    {
        V[QP][0]=V1[QP%6]<<(QP/6);
        V[QP][1]=V2[QP%6]<<(QP/6);
        V[QP][2]=V3[QP%6]<<(QP/6);
    }    
}

void init_quant()
{
    init_QPstep();
    init_MF();
    init_V();
}
//Quantisation for 4x4 blocks, mode 0 for intra, 1 for inter
uint8 quant_4x4(sint32* block, sint32* target, uint32 QP, uint32 mode) 
{

    uint32 i;
    uint32 qbits=(15+(QP/6));
    uint32 divider=mode?6:3;
    uint32 helper;
    uint32 qval=((1<<qbits)/divider);
    uint32 MFtable[16]={MF[QP][0],MF[QP][2],MF[QP][0],MF[QP][2],
                        MF[QP][2],MF[QP][1],MF[QP][2],MF[QP][1],
                        MF[QP][0],MF[QP][2],MF[QP][0],MF[QP][2],
                        MF[QP][2],MF[QP][1],MF[QP][2],MF[QP][1]};
 
    //Zij=round(Wij*(MF/2^qbits));
    for(i=0;i<16;i++)
    {
        //Defined in book page 192
        helper=((uint32)(abs(block[i])*MFtable[i]+qval))>>qbits;
        if(block[i]<0) target[i]=-1*helper;
        else target[i]=helper;
    }

    
    return 1;
}

//Inverse quantisation for 4x4 blocks
uint8 rescale_4x4(sint32* block, sint32* target, uint32 QP) 
{

    
    uint32 i;
    uint32 helper;

    uint32 Vtable[16]={V[QP][0],V[QP][2],V[QP][0],V[QP][2],
                        V[QP][2],V[QP][1],V[QP][2],V[QP][1],
                        V[QP][0],V[QP][2],V[QP][0],V[QP][2],
                        V[QP][2],V[QP][1],V[QP][2],V[QP][1]}; 

    for(i=0;i<16;i++)
    {
        target[i]=block[i]*Vtable[i]/**(1<<(QP/6))*/;
    }
    
    return 1;
}

//Quantisation for DC 4x4 blocks, mode 0 for intra, 1 for inter
uint8 quantDC_4x4(sint32* block, sint32* target, uint32 QP, uint32 mode) 
{

    uint32 i;

    uint32 qbits=(15+(QP/6));
    uint32 divider=mode?6:3;
    uint32 helper;
    uint32 qval=2*((1<<qbits)/divider);

    //|Zij| = (|Yij*MF[0]+qval) >> (qbits-1)
    for(i=0;i<16;i++)
    {
        //Defined in book page 194
        helper=((uint32)(abs(block[i])*MF[QP][0]+qval))>>(qbits-1);
        if(block[i]<0) target[i]=-1*helper;
        else target[i]=helper;
    }    
    return 1;
}


//Inverse quantisation for 4x4 DC blocks
uint8 rescaleDC_4x4(sint32* block, sint32* target, uint32 QP) 
{

    
    uint32 i;
    uint32 helper;

    if(QP>=12)
    {
        for(i=0;i<16;i++)
        {
            target[i]=block[i]*V[QP][0]*(1<<((QP/6)-2));
        }
    }
    else
    {
        for(i=0;i<16;i++)
        {
            helper=(abs(block[i])*V[QP][0]*(1<<(1-(QP/6))))>>(2-(QP/6));
            if(block[i]<0) target[i]=-1*helper;
            else target[i]=helper;
        }
    }
    
    return 1;
}


//Quantisation for DC 2x2 blocks, mode 0 for intra, 1 for inter
uint8 quantDC_2x2(sint32* block, sint32* target, uint32 QP, uint32 mode) 
{

    uint32 i;

    uint32 qbits=(15+(QP/6));
    uint32 divider=mode?6:3;
    uint32 helper;
    uint32 qval=2*((1<<qbits)/divider);

    //|Zij| = (|Yij*MF[0]+qval) >> (qbits-1)
    for(i=0;i<4;i++)
    {
        //Defined in book page 194
        helper=((uint32)(abs(block[i])*MF[QP][0]+qval))>>(qbits+1);
        if(block[i]<0) target[i]=-1*helper;
        else target[i]=helper;
    }    
    return 1;
}

//Inverse quantisation for 2x2 DC blocks
uint8 rescaleDC_2x2(sint32* block, sint32* target, uint32 QP) 
{

    
    uint32 i;
    uint32 helper;

    if(QP>=6)
    {
        for(i=0;i<4;i++)
        {
            target[i]=block[i]*V[QP][0]*(1<<((QP/6)-1));
        }
    }
    else
    {
        for(i=0;i<4;i++)
        {
            helper=(abs(block[i])*V[QP][0])>>1;
            if(block[i]<0) target[i]=-1*helper;
            else target[i]=helper;
        }
    }
    
    return 1;
}