t->qp_y = T264_MIN(t->qp_y, t->param.max_qp);
            }
            else
            {
                double tmp1, tmp2;
                double step;
                int32_t bits = rc->ideal_bits - t->header_bits;
                double mad = rc->a1 * rc->mad_p + rc->a2;
                int32_t qp;
                bits = T264_MAX(bits, (int32_t)(t->param.bitrate / t->param.framerate / 4));
                tmp1 = mad * mad * rc->x1 * rc->x1 + 4 * rc->x2 * mad * bits;
                tmp2 = sqrt(tmp1) - rc->x1 * mad;
                if (rc->x2 < 0.000001 || tmp1 < 0.000001 ||  tmp2 <= 0.000001)
                    step = rc->x1 * mad / bits;
                else
                    step = rc->x2 * mad * 2 / tmp2;

                qp = qstep2qp(step);
                qp = T264_MIN(rc->qp_p2 + 2, qp);
                qp = T264_MIN(t->param.max_qp, qp);
                qp = T264_MAX(rc->qp_p2 - 2, qp);
                t->qp_y = T264_MAX(t->param.min_qp, qp);
            }
        }
        rc->qp_p1 = rc->qp_p2;
        rc->qp_p2 = t->qp_y;
    }
    else if (t->slice_type == SLICE_B)
    {
        if (t->param.b_num == 1)
        {
            if (rc->qp_p1 == rc->qp_p2)
                t->qp_y += 2;
            else
                t->qp_y = (rc->qp_p1 + rc->qp_p2) / 2 + 1;
            
            t->qp_y = clip3(t->qp_y, t->param.min_qp, t->param.max_qp);
        }
        else
        {
            int32_t step_size;
            if (rc->qp_p2 - rc->qp_p1 <= -2 * t->param.b_num - 3)
                step_size = -3;
            else if (rc->qp_p2 - rc->qp_p1 == -2 * t->param.b_num - 2)
                step_size = -2;
            else if (rc->qp_p2 - rc->qp_p1 == -2 * t->param.b_num - 1)
                step_size = -1;
            else if (rc->qp_p2 - rc->qp_p1 == -2 * t->param.b_num - 0)
                step_size = 0;
            else if (rc->qp_p2 - rc->qp_p1 == -2 * t->param.b_num + 1)
                step_size = 1;
            else
                step_size = 2;

            t->qp_y = rc->qp_p1 + step_size;
            //t->qp_y += T264_MIN(2 * rc->b_no, 
            //    T264_MAX(-2 * rc->b_no, rc->b_no * (rc->qp_p2 - rc->qp_p1) / (t->param.b_num - 1)));
            t->qp_y = clip3(t->qp_y, t->param.min_qp, t->param.max_qp);
        }
    }
}

static void __inline
remove_outlier(T264_t* t, int8_t valid[20], T264_rc_t* rc)
{
    double error[20];
    int32_t i;
    double std = 0.0;
    double threshold;

    for(i = 0 ; i < rc->window_p ; i ++)
    {
        error[i] = rc->x1 / rc->qp[i] + rc->x2 / (rc->qp[i] * rc->rp[i]) - rc->qp[i];
        std += error[i] * error[i];
    }

    threshold = rc->window_p == 2 ? 0 : sqrt(std / rc->window_p);

    for(i = 1 ; i < rc->window_p ; i ++)
    {
        if (error[i] > threshold)
            valid[i] = FALSE;
    }
}

static void __inline
remove_mad_outlier(T264_t* t, int8_t valid[20], T264_rc_t* rc)
{
    double error[20];
    int32_t i;
    double std = 0.0;
    double threshold;

    for(i = 0 ; i < rc->mad_window_p ; i ++)
    {
        error[i] = rc->a1 * rc->mad[i + 1] + rc->a2 - rc->mad[i];
        std += error[i] * error[i];
    }

    threshold = rc->mad_window_p == 2 ? 0 : sqrt(std / rc->mad_window_p);

    for(i = 1 ; i < rc->mad_window_p ; i ++)
    {
        if (error[i] > threshold)
            valid[i] = FALSE;
    }
}

void
mad_model_estimator(T264_t* t, int8_t valid[20], int32_t window, T264_rc_t* rc)
{
    int32_t real_window = 0, i;
    int8_t estimate_x2 = FALSE;
    double a00 = 0.0, a01 = 0.0, a10 = 0.0, a11 = 0.0, b0 = 0.0, b1 = 0.0;
    double mad;
    double matrix_value;

    for(i = 0 ; i < window ; i ++)
    {
        if (valid[i])
        {
            real_window ++;
            mad = rc->mad[i];
        }
    }

    rc->a1 = rc->a2 = 0.0;
    for(i = 0 ; i < window ; i ++)
    {
        if (valid[i])
        {
            rc->a1 += rc->mad[i] / (rc->mad[i + 1] * real_window);
            if (ABS(mad - rc->mad[i]) < 0.00001)
                estimate_x2 = TRUE;
        }
    }

    if (real_window >= 1 && estimate_x2)
    {
        for(i = 0 ; i < window ; i ++)
        {
            if (valid[i])
            {
                a00 += 1.0;
                a01 += rc->mad[i + 1];
                a10 = a01;
                a11 += rc->mad[i + 1] * rc->mad[i + 1];
                b0 += rc->mad[i];
                b1 += rc->mad[i] * rc->mad[i + 1];
            }
        }

        matrix_value = a00 * a11 - a01 * a10;
        if (ABS(matrix_value) > 0.000001)
        {
            rc->a2 = (b0 * a11 - b1 * a01) / matrix_value;
            rc->a1 = (b1 * a00 - b0 * a10) / matrix_value;
        }
        else
        {
            rc->a1 = b0 / a01;
            rc->a2 = 0.0;
        }
    }
}

void
rc_update_mad_model(T264_t* t, T264_rc_t* rc)
{
    int32_t i;
    int32_t window;
    double mad;
    int8_t valid[20];

    if (t->slice_type == SLICE_P)
    {
        mad = ((double)t->sad_all) / ((double)(t->width * t->height));
        // update x1, x2
        for (i = 19 ; i > 0 ; i --)
        {
            rc->mad[i] = rc->mad[i - 1];
        }
        rc->mad[0] = mad;
        window = (int32_t)(mad > rc->mad_p ? rc->mad_p / mad * 20 : mad / rc->mad_p * 20);
        window = clip3(1, rc->p_no + 1, window);
        window = T264_MIN(window, rc->mad_window_p + 1);
        window = T264_MIN(window, 20);
        rc->mad_window_p = window;
        memset(valid, TRUE, sizeof(valid));
        mad_model_estimator(t, valid, window, rc);
        remove_mad_outlier(t, valid, rc);
        mad_model_estimator(t, valid, window, rc);
    }
}

void
quad_model_estimator(T264_t* t, int8_t valid[20], int32_t window, T264_rc_t* rc)
{
    int32_t real_window = 0, i;
    int8_t estimate_x2 = FALSE;
    double a00 = 0.0, a01 = 0.0, a10 = 0.0, a11 = 0.0, b0 = 0.0, b1 = 0.0;
    double qp;
    double matrix_value;

    for(i = 0 ; i < window ; i ++)
    {
        if (valid[i])
        {
            real_window ++;
            qp = rc->qp[i];
        }
    }

    rc->x1 = rc->x2 = 0.0;
    for(i = 0 ; i < window ; i ++)
    {
        if (valid[i])
        {
            rc->x1 += rc->qp[i] * rc->rp[i] / real_window;
            if (qp != rc->qp[i])
                estimate_x2 = TRUE;
        }
    }

    if (real_window >= 1 && estimate_x2)
    {
        for(i = 0 ; i < window ; i ++)
        {
            if (valid[i])
            {
                a00 += 1.0;
                a01 += 1.0 / rc->qp[i];
                a10 = a01;
                a11 += 1.0 / (rc->qp[i] * rc->rp[i]);
                b0 += rc->qp[i] * rc->rp[i];
                b1 += rc->rp[i];
            }
        }

        matrix_value = a00 * a11 - a01 * a10;
        if (ABS(matrix_value) > 0.000001)
        {
            rc->x1 = (b0 * a11 - b1 * a01) / matrix_value;
            rc->x2 = (b1 * a00 - b0 * a10) / matrix_value;
        }
        else
        {
            rc->x1 = b0 / a00;
            rc->x2 = 0.0;
        }
    }
}

static void 
rc_update_quad_model(T264_t* t, T264_rc_t* rc)
{
    int32_t i;
    int32_t window;
    double mad;
    int8_t valid[20];

    if (t->slice_type == SLICE_P)
    {
        mad = ((double)t->sad_all) / ((double)(t->width * t->height));
        // update x1, x2
        for (i = 19 ; i > 0 ; i --)
        {
            rc->qp[i] = rc->qp[i - 1];
            rc->rp[i] = rc->rp[i - 1];
        }
        rc->qp[0] = t->qp_y;
        rc->rp[0] = ((double)(t->frame_bits - t->header_bits)) / mad;
        window = (int32_t)(mad > rc->mad_p ? rc->mad_p / mad * 20 : mad / rc->mad_p * 20);
        window = clip3(1, rc->p_no + 1, window);
        window = T264_MIN(window, rc->window_p + 1);
        window = T264_MIN(window, 20);
        rc->window_p = window;
        if (window > 0)
        {
            memset(valid, TRUE, sizeof(valid));
            quad_model_estimator(t, valid, window, rc);
            remove_outlier(t, valid, rc);
            quad_model_estimator(t, valid, window, rc);
        }
        if (rc->p_no > 0)
        {
            // update a1, a2
            rc_update_mad_model(t, rc);
        }
        else
        {
            rc->mad[0] = mad;
        }
        rc->mad_p = mad;
    }
}

static void
rc_proc(T264_t* t, void* data, int32_t state)
{
    T264_rc_t* rc = data;
    switch (state) 
    {
    case STATE_BEFOREPIC:
        rc_init_pic(t, rc);
        break;
    case STATE_AFTERPIC:
        rc_update_pic(t, rc);
        break;
    case STATE_BEFOREGOP:
        rc_init_gop(t, rc);
        break;
    case STATE_BEFORESEQ:
        rc_init_seq(t, rc);
        break;
    default:
        break;
    }
}

void
rc_create(T264_t* t, T264_plugin_t* plugin)
{
    T264_rc_t* handle = malloc(sizeof(T264_rc_t));
    memset(handle, 0, sizeof(T264_rc_t));

    plugin->handle = handle;
    plugin->proc = rc_proc;
    plugin->close = rc_destroy;
    plugin->ret = 0;
}

void
rc_destroy(T264_t* t, T264_plugin_t* plugin)
{
    free(plugin->handle);
}