/*
 * Copyright @ 2015 Atlassian Pty Ltd
 *
 * Licensed under the Apache License, Version 2.0 (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.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.jitsi.impl.neomedia.codec.audio.silk;

import static org.jitsi.impl.neomedia.codec.audio.silk.Define.*;

/**
 * Residual energy.
 *
 * @author Dingxin Xu
 */
public class ResidualEnergyFLP
{


    static final int  MAX_ITERATIONS_RESIDUAL_NRG = 10;
    static final float  REGULARIZATION_FACTOR = 1e-8f;


    /**
     * Residual energy: nrg = wxx - 2 * wXx * c + c' * wXX * c.
     * @param c Filter coefficients
     * @param c_offset offset of valid data.
     * @param wXX Weighted correlation matrix, reg. out
     * @param wXX_offset offset of valid data.
     * @param wXx Weighted correlation vector
     * @param wxx Weighted correlation value
     * @param D Dimension
     * @return Weighted residual energy
     */
    static float SKP_Silk_residual_energy_covar_FLP(           /* O    Weighted residual energy                */
        final float                 []c,                 /* I    Filter coefficients                     */
              int                   c_offset,
              float                 []wXX,               /* I/O  Weighted correlation matrix, reg. out   */
              int                   wXX_offset,
        final float                 []wXx,               /* I    Weighted correlation vector             */
        final float                 wxx,                /* I    Weighted correlation value              */
        final int                   D                   /* I    Dimension                               */
    )
    {
        int   i, j, k;
        float tmp, nrg = 0, regularization;

        /* Safety checks */
        assert( D >= 0 );

        regularization = REGULARIZATION_FACTOR * ( wXX[ wXX_offset + 0 ] + wXX[ wXX_offset + D * D - 1 ] );
        for( k = 0; k < MAX_ITERATIONS_RESIDUAL_NRG; k++ ) {
            nrg = wxx;

            tmp = 0.0f;
            for( i = 0; i < D; i++ ) {
                tmp += wXx[ i ] * c[ c_offset + i ];
            }
            nrg -= 2.0f * tmp;

            /* compute c' * wXX * c, assuming wXX is symmetric */
            for( i = 0; i < D; i++ ) {
                tmp = 0.0f;
                for( j = i + 1; j < D; j++ ) {
//                    tmp += matrix_c_ptr( wXX, i, j, D ) * c[ j ];
                    tmp += wXX[wXX_offset + i+j*D] * c[c_offset + j];
                }
//                nrg += c[ i ] * ( 2.0f * tmp + matrix_c_ptr( wXX, i, i, D ) * c[ i ] );
                nrg += c[ c_offset + i ] * ( 2.0f * tmp + wXX[wXX_offset + i + D*i] * c[ c_offset + i ] );
            }
            if( nrg > 0 ) {
                break;
            } else {
                /* Add white noise */
                for( i = 0; i < D; i++ ) {
//                    matrix_c_ptr( wXX, i, i, D ) +=  regularization;
                    wXX[wXX_offset + i + D*i] += regularization;
                }
                /* Increase noise for next run */
                regularization *= 2.0f;
            }
        }
        if( k == MAX_ITERATIONS_RESIDUAL_NRG ) {
            assert( nrg == 0 );
            nrg = 1.0f;
        }

        return nrg;
    }

    /**
     * Calculates residual energies of input subframes where all subframes have LPC_order
     * of preceeding samples
     * @param nrgs Residual energy per subframe
     * @param x Input signal
     * @param a AR coefs for each frame half
     * @param gains Quantization gains
     * @param subfr_length Subframe length
     * @param LPC_order LPC order
     */
    static void SKP_Silk_residual_energy_FLP(
              float nrgs[],                     /* O    Residual energy per subframe    */
        final float x[],                        /* I    Input signal                    */
        final float a[][ ],    /* I    AR coefs for each frame half    */
        final float gains[],                    /* I    Quantization gains              */
        final int   subfr_length,               /* I    Subframe length                 */
        final int   LPC_order                   /* I    LPC order                       */
    )
    {
        int         shift;
//        SKP_float       *LPC_res_ptr, LPC_res[ ( MAX_FRAME_LENGTH + NB_SUBFR * MAX_LPC_ORDER ) / 2 ];
        float       LPC_res_ptr[], LPC_res[] = new float[ ( MAX_FRAME_LENGTH + NB_SUBFR * MAX_LPC_ORDER ) / 2 ];

//        LPC_res_ptr = LPC_res + LPC_order;
        LPC_res_ptr = LPC_res;
        int LPC_res_ptr_offset = LPC_order;
        shift = LPC_order + subfr_length;

        /* Filter input to create the LPC residual for each frame half, and measure subframe energies */
        LPCAnalysisFilterFLP.SKP_Silk_LPC_analysis_filter_FLP( LPC_res, a[ 0 ], x, 0 + 0 * shift, 2 * shift, LPC_order );
        nrgs[ 0 ] = (float) ( gains[ 0 ] * gains[ 0 ] * EnergyFLP.SKP_Silk_energy_FLP( LPC_res_ptr, LPC_res_ptr_offset + 0 * shift, subfr_length ) );
        nrgs[ 1 ] = (float) ( gains[ 1 ] * gains[ 1 ] * EnergyFLP.SKP_Silk_energy_FLP( LPC_res_ptr, LPC_res_ptr_offset + 1 * shift, subfr_length ) );

        LPCAnalysisFilterFLP.SKP_Silk_LPC_analysis_filter_FLP( LPC_res, a[ 1 ], x, 0 + 2 * shift, 2 * shift, LPC_order );
        nrgs[ 2 ] = ( float )( gains[ 2 ] * gains[ 2 ] * EnergyFLP.SKP_Silk_energy_FLP( LPC_res_ptr, LPC_res_ptr_offset + 0 * shift, subfr_length ) );
        nrgs[ 3 ] = ( float )( gains[ 3 ] * gains[ 3 ] * EnergyFLP.SKP_Silk_energy_FLP( LPC_res_ptr, LPC_res_ptr_offset + 1 * shift, subfr_length ) );
    }
}
