/*****************************************************************************
*
*  T264 AVC CODEC
*
*  Copyright(C) 2004-2005 llcc <lcgate1@yahoo.com.cn>
*               2004-2005 visionany <visionany@yahoo.com.cn>
*
*  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., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
*
****************************************************************************/

#include "stdio.h"
#include "stdlib.h"
#include "T264.h"
#include "ratecontrol.h"
#include "math.h"
#ifndef CHIP_DM642
#include "memory.h"
#endif

typedef struct
{
    // frame numbers per gop
    int32_t gop;
    int32_t np;
    int32_t nb;
    int32_t qp_sum;
    // the number in current gop, based on 0
    int32_t p_no;
    int32_t b_no;
    // prev qp1, qp2
    int32_t qp_p1;
    int32_t qp_p2;
    double deltap;
    // fluid Flow Traffic Model
    int32_t bc;
    // linear Model
    double a1;
    double a2;
    // model
    double x1;
    double x2;
    double qp[20];
    double rp[20];
    double mad[20];
    int32_t window_p;
    int32_t mad_window_p;
    // remaining bits in gop
    int32_t gop_bits;
    double tbl;
    double gamma;
    double beta;
    double theta;
    double wp;
    double wb;
    double AWp;
    double AWb;
    int32_t ideal_bits;
    double mad_p;
} T264_rc_t;

void rc_init_seq(T264_t* t, T264_rc_t* rc);
void rc_init_gop(T264_t* t, T264_rc_t* rc);
void rc_init_pic(T264_t* t, T264_rc_t* rc);
void rc_update_pic(T264_t* t, T264_rc_t* rc);
// frame level
void rc_update_qp(T264_t* t, T264_rc_t* rc);
// quadratic model
void rc_update_quad_model(T264_t* t, T264_rc_t* rc);

void 
rc_init_seq(T264_t* t, T264_rc_t* rc)
{
    double bpp, L1, L2, L3;

    rc->gop = T264_MIN(t->param.idrframe, t->param.iframe);
    rc->np = rc->gop / (1 + t->param.b_num) - 1;
    rc->nb = rc->gop - rc->np - 1;
    rc->bc = 0;
    rc->a1 = 1.0;
    rc->a2 = 0.0;
    rc->x1 = t->param.bitrate;

    if (rc->nb > 0)
    {
        rc->gamma = 0.25;
        rc->beta = 0.9;
        rc->theta = 1.3636;
    }
    else
    {
        rc->gamma = 0.5;
        rc->beta = 0.75;
    }

    if (t->param.qp == 0)
    {
        bpp = ((double)t->param.bitrate) / (t->param.framerate * t->param.width * t->param.height);
        if (t->param.width == 176)
        {
            L1 = 0.1;
            L2 = 0.3;
            L3 = 0.6;
        }
        else if (t->param.width == 352)
        {
            L1 = 0.2;
            L2 = 0.6;
            L3 = 1.2;
        }
        else
        {
            L1 = 0.6;
            L2 = 1.4;
            L3 = 2.4;
        }

        // first gop first i, p
        if (bpp <= L1)
        {
            t->qp_y = 30;
        }
        else if (bpp <= L2)
        {
            t->qp_y = 25;
        }
        else if (bpp <= L3)
        {
            t->qp_y = 20;
        }
        else
        {
            t->qp_y = 10;
        }
    }
}

void 
rc_init_gop(T264_t* t, T264_rc_t* rc)
{
    rc->gop_bits = (int32_t)(rc->gop * t->param.bitrate / (t->param.framerate) - rc->bc);
    if (t->frame_id != 0)
    {
        // JVTH0014 say to do so
        // t->qp_y = rc->qp_sum / rc->np + 8 * rc->bc / rc->gop_bits - T264_MIN(2, rc->gop / 15);
        // JM does this way
        t->qp_y = rc->qp_sum / rc->np - T264_MIN(2, rc->gop / 15);
        if (rc->gop != 1)
        {
            if (t->qp_y > rc->qp_p2 - 2)
                t->qp_y --;
            t->qp_y = clip3(t->qp_y, rc->qp_p2 - 2, rc->qp_p2 + 2);
        }
        t->qp_y = clip3(t->qp_y, t->param.min_qp, t->param.max_qp);
    }

    rc->qp_sum = 0;
    rc->p_no = 0;
    rc->b_no = 0;
    rc->qp_p2 = t->qp_y;
}

void 
rc_init_pic(T264_t* t, T264_rc_t* rc)
{
    int32_t f1, f2, f;

    if (t->slice_type == SLICE_P)
    {
        if (rc->p_no > 0)
        {
            // Step 1.1 Determination of target buffer occupancy
            if (rc->p_no == 1)
            {
                rc->deltap = ((double)rc->bc) / (double)(rc->np - 1);
                rc->tbl = rc->bc;
                rc->AWp = rc->wp;
                rc->AWb = rc->wb;
            }
            else if (rc->p_no < 8)
            {
                rc->AWp = rc->wp * (rc->p_no - 1) / (rc->p_no) + rc->AWp/ (rc->p_no);
            }
            else
            {
                rc->AWp = rc->wp / 8 + 7 * rc->AWp / 8;
            }
            
            rc->tbl -= rc->deltap;
            if (t->param.b_num > 0)
            {
                rc->tbl += rc->AWp * (t->param.b_num + 1) * t->param.bitrate / (t->param.framerate * (rc->AWp + rc->AWb * t->param.b_num))
                    - t->param.bitrate / t->param.framerate;
            }

            // Step 1.2 Microscopic control (target bit rate computation).
            f1 = (int32_t)(t->param.bitrate / t->param.framerate + rc->gamma * (rc->tbl - rc->bc));
            f1 = T264_MAX(0, f1);
            f2 = (int32_t)(rc->wp * rc->gop_bits / (rc->wp * (rc->np - rc->p_no) + rc->wb * (rc->nb - rc->b_no)));
            //f2 = rc->gop_bits / (rc->np - rc->p_no);
            f = (int32_t)(rc->beta * f1 + (1 - rc->beta) * f2);

            rc->ideal_bits = (int32_t)(f * (1 - t->param.b_num * 0.05));
            // HRD consideration ??
        }
    }
    else if (t->slice_type == SLICE_B)
    {
        if (rc->b_no > 0)
        {
            if (rc->b_no == 1)
            {
                rc->AWb = rc->wb;
            }
            else if (rc->b_no < 8)
            {
                rc->AWb = rc->wb * (rc->b_no - 1) / rc->b_no + rc->AWb/ rc->b_no;
            }
            else
            {
                rc->AWb = rc->wb / 8 + 7 * rc->AWb / 8;
            }
        }
    }
    rc_update_qp(t, rc);
}

static void 
rc_update_pic(T264_t* t, T264_rc_t* rc)
{
    int32_t X;

    rc_update_quad_model(t, rc);
    rc->gop_bits -= t->frame_bits;
    rc->bc += (int32_t)(t->frame_bits - t->param.bitrate / t->param.framerate);

    X = (int32_t)t->qp_y * t->frame_bits;
    
    if (t->slice_type == SLICE_P)
    {
        rc->qp_sum += t->qp_y;
        rc->p_no ++;
        rc->wp = X;
        // compute mad
    }
    else if (t->slice_type == SLICE_B)
    {
        rc->b_no ++;
        rc->wb = X / rc->theta;
    }
}

double
qp2qstep( int32_t qp)
{
    int32_t i; 
    double qstep;
    static const double QP2QSTEP[6] = 
    {0.625, 0.6875, 0.8125, 0.875, 1.0, 1.125};

    qstep = QP2QSTEP[qp % 6];
    for(i = 0; i< (qp/6) ; i ++)
        qstep *= 2;

    return qstep;
}

int32_t
qstep2qp(double qstep)
{
    int32_t q_per = 0, q_rem = 0;

    if( qstep < qp2qstep(0))
        return 0;
    else if (qstep > qp2qstep(51))
        return 51;

    while( qstep > qp2qstep(5))
    {
        qstep /= 2;
        q_per += 1;
    }

    if (qstep <= (0.625+0.6875)/2) 
    {
        qstep = 0.625;
        q_rem = 0;
    }
    else if (qstep <= (0.6875+0.8125)/2)
    {
        qstep = 0.6875;
        q_rem = 1;
    }
    else if (qstep <= (0.8125+0.875)/2)
    {
        qstep = 0.8125;
        q_rem = 2;
    }
    else if (qstep <= (0.875+1.0)/2)
    {
        qstep = 0.875;
        q_rem = 3;
    }
    else if (qstep <= (1.0+1.125)/2)
    {
        qstep = 1.0;  
        q_rem = 4;
    }
    else 
    {
        qstep = 1.125;
        q_rem = 5;
    }

    return (q_per * 6 + q_rem);
}

void 
rc_update_qp(T264_t* t, T264_rc_t* rc)
{
    if (t->slice_type == SLICE_P)
    {
        if (rc->p_no != 0)
        {
            if (rc->ideal_bits < 0)
            {
                t->qp_y += 2;