/*
 * 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.*;

public class SolveLSFLP
{
    /**
     * Function to solve linear equation Ax = b, when A is a MxM
     * symmetric square matrix - using LDL factorisation.
     * @param A Symmetric square matrix, out: reg.
     * @param A_offset offset of valid data.
     * @param M Size of matrix
     * @param b Pointer to b vector
     * @param x Pointer to x solution vector
     * @param x_offset offset of valid data.
     */
    static void SKP_Silk_solve_LDL_FLP(
              float                 []A,                 /* I/O  Symmetric square matrix, out: reg.      */
              int                   A_offset,
        final int                   M,                  /* I    Size of matrix                          */
        final float                 []b,                 /* I    Pointer to b vector                     */
              float                 []x,                  /* O    Pointer to x solution vector            */
              int                   x_offset
    )
    {
        int i;
//        float L[][] = new float[MAX_MATRIX_SIZE][MAX_MATRIX_SIZE];
//TODO:change L from two dimension to one dimension.
        float L_tmp[] = new float[MAX_MATRIX_SIZE*MAX_MATRIX_SIZE];
        float T[] = new float[MAX_MATRIX_SIZE];
        float Dinv[] = new float[MAX_MATRIX_SIZE];// inverse diagonal elements of D

        assert( M <= MAX_MATRIX_SIZE );

        /***************************************************
        Factorize A by LDL such that A = L*D*(L^T),
        where L is lower triangular with ones on diagonal
        ****************************************************/
//        SKP_Silk_LDL_FLP( A, M, &L[ 0 ][ 0 ], Dinv );
        SKP_Silk_LDL_FLP(A, A_offset, M, L_tmp, Dinv);

        /****************************************************
        * substitute D*(L^T) = T. ie:
        L*D*(L^T)*x = b => L*T = b <=> T = inv(L)*b
        ******************************************************/
//        SKP_Silk_SolveWithLowerTriangularWdiagOnes_FLP( &L[ 0 ][ 0 ], M, b, T );
        SKP_Silk_SolveWithLowerTriangularWdiagOnes_FLP( L_tmp, M, b, T );

        /****************************************************
        D*(L^T)*x = T <=> (L^T)*x = inv(D)*T, because D is
        diagonal just multiply with 1/d_i
        ****************************************************/
        for( i = 0; i < M; i++ ) {
            T[ i ] = T[ i ] * Dinv[ i ];
        }
        /****************************************************
        x = inv(L') * inv(D) * T
        *****************************************************/
//        SKP_Silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP( &L[ 0 ][ 0 ], M, T, x );
        SKP_Silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP( L_tmp, M, T, x, x_offset );

    }

    /**
     * Function to solve linear equation (A^T)x = b, when A is a MxM lower
     * triangular, with ones on the diagonal. (ie then A^T is upper triangular)
     * @param L Pointer to Lower Triangular Matrix
     * @param M Dim of Matrix equation
     * @param b b Vector
     * @param x x Vector
     * @param x_offset offset of valid data.
     */
    static void SKP_Silk_SolveWithUpperTriangularFromLowerWdiagOnes_FLP(
        final float     []L,     /* (I) Pointer to Lower Triangular Matrix */
        int             M,      /* (I) Dim of Matrix equation */
        final float     []b,     /* (I) b Vector */
        float           []x,      /* (O) x Vector */
        int             x_offset
    )
    {
//        SKP_int   i, j;
//        SKP_float temp;
//        const SKP_float *ptr1;
        int i,j;
        float temp;
//TODO:ignore const
        float []ptr1;
        int ptr1_offset;

        for( i = M - 1; i >= 0; i-- ) {
//            ptr1 =  matrix_adr( L, 0, i, M );
            ptr1 = L;
            ptr1_offset = i;
            temp = 0;
            for( j = M - 1; j > i ; j-- ) {
                temp += ptr1[ ptr1_offset + j * M ] * x[ x_offset + j ];
            }
            temp = b[ i ] - temp;
            x[ x_offset + i ] = temp;
        }
    }

    /**
     * Function to solve linear equation Ax = b, when A is a MxM lower
     * triangular matrix, with ones on the diagonal.
     * @param L Pointer to Lower Triangular Matrix
     * @param M Pointer to Lower Triangular Matrix
     * @param b b Vector
     * @param x x Vector
     */
    static void SKP_Silk_SolveWithLowerTriangularWdiagOnes_FLP(
        final float     []L,     /* (I) Pointer to Lower Triangular Matrix */
        int             M,      /* (I) Pointer to Lower Triangular Matrix */
        final float     []b,     /* (I) b Vector */
        float           []x      /* (O) x Vector */
    )
    {
//        SKP_int   i, j;
//        SKP_float temp;
//        const SKP_float *ptr1;
        int i,j;
        float temp;
//TODO:ignore const
        float []ptr1;
        int ptr1_offset;

        for( i = 0; i < M; i++ ) {
//            ptr1 =  matrix_adr( L, i, 0, M );
            ptr1 = L;
            ptr1_offset = i*M;
            temp = 0;
            for( j = 0; j < i; j++ ) {
                temp += ptr1[ ptr1_offset + j ] * x[ j ];
            }
            temp = b[ i ] - temp;
            x[ i ] = temp;
        }
    }

    /**
     * LDL Factorisation. Finds the upper triangular matrix L and the diagonal
     * Matrix D (only the diagonal elements returned in a vector)such that
     * the symmetric matric A is given by A = L*D*L'.
     * @param A Pointer to Symetric Square Matrix
     * @param A_offset offset of valid data.
     * @param M Size of Matrix
     * @param L Pointer to Square Upper triangular Matrix
     * @param Dinv Pointer to vector holding the inverse diagonal elements of D
     */
    static void SKP_Silk_LDL_FLP(
        float           []A,      /* (I/O) Pointer to Symetric Square Matrix */
        int             A_offset,
        int             M,       /* (I) Size of Matrix */
        float           []L,      /* (I/O) Pointer to Square Upper triangular Matrix */
        float           []Dinv    /* (I/O) Pointer to vector holding the inverse diagonal elements of D */
    )
    {
/*        SKP_int i, j, k, loop_count, err = 1;
        SKP_float *ptr1, *ptr2;
        double temp, diag_min_value;
        SKP_float v[ MAX_MATRIX_SIZE ], D[ MAX_MATRIX_SIZE ]; // temp arrays
*/
        int i, j, k, loop_count, err = 1;
        float ptr1[], ptr2[];
        int ptr1_offset, ptr2_offset;
        double temp, diag_min_value;
        float v[] = new float[ MAX_MATRIX_SIZE ], D[] = new float[ MAX_MATRIX_SIZE ]; // temp arrays

        assert( M <= MAX_MATRIX_SIZE );

        diag_min_value = DefineFLP.FIND_LTP_COND_FAC * 0.5f * ( A[ A_offset + 0 ] + A[ A_offset + M * M - 1 ] );
        for( loop_count = 0; loop_count < M && err == 1; loop_count++ ) {
            err = 0;
            for( j = 0; j < M; j++ ) {
//                ptr1 = matrix_adr( L, j, 0, M );
//                temp = matrix_ptr( A, j, j, M ); // element in row j column j
                ptr1 = L;
                ptr1_offset = j*M + 0;
                temp = A[A_offset + j*M + j];
                for( i = 0; i < j; i++ ) {
                    v[ i ] = ptr1[ ptr1_offset + i ] * D[ i ];
                    temp  -= ptr1[ ptr1_offset + i ] * v[ i ];
                }
                if( temp < diag_min_value ) {
                    /* Badly conditioned matrix: add white noise and run again */
                    temp = ( loop_count + 1 ) * diag_min_value - temp;
                    for( i = 0; i < M; i++ ) {
//                        matrix_ptr( A, i, i, M ) += ( SKP_float )temp;
                        A[A_offset + i*M+i] += temp;
                    }
                    err = 1;
                    break;
                }
                D[ j ]    = ( float )temp;
                Dinv[ j ] = ( float )( 1.0f / temp );
//                matrix_ptr( L, j, j, M ) = 1.0f;
                L[j*M+j] = 1.0f;

//                ptr1 = matrix_adr( A, j, 0, M );
//                ptr2 = matrix_adr( L, j + 1, 0, M);
                ptr1 = A;
                ptr1_offset = A_offset + j*M;
                ptr2 = L;
                ptr2_offset = (j+1)*M;

                for( i = j + 1; i < M; i++ ) {
                    temp = 0.0;
                    for( k = 0; k < j; k++ ) {
                        temp += ptr2[ ptr2_offset + k ] * v[ k ];
                    }
//                    matrix_ptr( L, i, j, M ) = ( SKP_float )( ( ptr1[ i ] - temp ) * Dinv[ j ] );
                    L[i*M+j] = (float) ( ( ptr1[ ptr1_offset + i ] - temp ) * Dinv[ j ] );
                    ptr2_offset += M; // go to next column
                }
            }
        }
        assert( err == 0 );
    }
}
