/*
 * Copyright (c) 2009-2020, Peter Abeles. All Rights Reserved.
 *
 * This file is part of Efficient Java Matrix Library (EJML).
 *
 * 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.ejml.dense.block;

import org.ejml.data.DMatrixRBlock;

/**
 * <p>
 * Contains triangular solvers for inner blocks of a {@link DMatrixRBlock}.
 * </p>
 *
 * Algorithm for lower triangular inverse:<br>
 *
 * <pre>
 * for i=1:m
 *     for j=1:i-1
 *         val = 0
 *         for k=j:i-1
 *             val = val - L(i,k) * X(k,j)
 *         end
 *         x(i,j) = val / L(i,i)
 *     end
 *     x(i,i) = 1 / L(i,i)
 * end
 * </pre>
 *
 * @author Peter Abeles
 */
public class InnerTriangularSolver_DDRB {

    /**
     * <p>
     * Inverts a square lower triangular matrix:  L = L<sup>-1</sup>
     * </p>
     *
     * @param L Lower triangular matrix being inverted. Not modified.
     * @param L_inv Where the inverse is stored. Can be the same as L. Modified.
     * @param m The number of rows and columns.
     * @param offsetL which index does the L matrix start at.
     * @param offsetL_inv which index does the L_inv matrix start at.
     */
    public static void invertLower( double[] L,
                                    double[] L_inv,
                                    int m,
                                    int offsetL,
                                    int offsetL_inv ) {
        for (int i = 0; i < m; i++) {
            double L_ii = L[offsetL + i*m + i];
            for (int j = 0; j < i; j++) {
                double val = 0;
                for (int k = j; k < i; k++) {
                    val += L[offsetL + i*m + k]*L_inv[offsetL_inv + k*m + j];
                }
                L_inv[offsetL_inv + i*m + j] = -val/L_ii;
            }
            L_inv[offsetL_inv + i*m + i] = 1.0/L_ii;
        }
    }

    /**
     * <p>
     * Inverts a square lower triangular matrix:  L = L<sup>-1</sup>
     * </p>
     *
     * @param L Lower triangular matrix being inverted. Over written with inverted matrix. Modified.
     * @param m The number of rows and columns.
     * @param offsetL which index does the L matrix start at.
     */
    public static void invertLower( double[] L,
                                    int m,
                                    int offsetL ) {
        for (int i = 0; i < m; i++) {
            double L_ii = L[offsetL + i*m + i];
            for (int j = 0; j < i; j++) {
                double val = 0;
                for (int k = j; k < i; k++) {
                    val += L[offsetL + i*m + k]*L[offsetL + k*m + j];
                }
                L[offsetL + i*m + j] = -val/L_ii;
            }
            L[offsetL + i*m + i] = 1.0/L_ii;
        }
    }

    /**
     * <p>
     * Solves for non-singular lower triangular matrices using forward substitution.
     * <br>
     * B = L<sup>-1</sup>B<br>
     * <br>
     * where B is a (m by n) matrix, L is a lower triangular (m by m) matrix.
     * </p>
     *
     * @param L An m by m non-singular lower triangular matrix. Not modified.
     * @param b An m by n matrix. Modified.
     * @param m size of the L matrix
     * @param n number of columns in the B matrix.
     * @param strideL number of elements that need to be added to go to the next row in L
     * @param offsetL initial index in L where the matrix starts
     * @param offsetB initial index in B where the matrix starts
     */
    public static void solveL( double[] L, double[] b,
                               int m, int n,
                               int strideL, int offsetL, int offsetB ) {
        for (int j = 0; j < n; j++) {
            for (int i = 0; i < m; i++) {
                double sum = b[offsetB + i*n + j];
                for (int k = 0; k < i; k++) {
                    sum -= L[offsetL + i*strideL + k]*b[offsetB + k*n + j];
                }
                b[offsetB + i*n + j] = sum/L[offsetL + i*strideL + i];
            }
        }
    }

    /**
     * <p>
     * Solves for non-singular transposed lower triangular matrices using backwards substitution:
     * <br>
     * B = L<sup>-T</sup>B<br>
     * <br>
     * where B is a (m by n) matrix, L is a lower triangular (m by m) matrix.
     * </p>
     *
     * @param L An m by m non-singular lower triangular matrix. Not modified.
     * @param b An m by n matrix. Modified.
     * @param m size of the L matrix
     * @param n number of columns in the B matrix.
     * @param strideL number of elements that need to be added to go to the next row in L
     * @param offsetL initial index in L where the matrix starts
     * @param offsetB initial index in B where the matrix starts
     */
    public static void solveTransL( double[] L, double[] b,
                                    int m, int n,
                                    int strideL, int offsetL, int offsetB ) {
        for (int j = 0; j < n; j++) {
            for (int i = m - 1; i >= 0; i--) {
                double sum = b[offsetB + i*n + j];
                for (int k = i + 1; k < m; k++) {
                    sum -= L[offsetL + k*strideL + i]*b[offsetB + k*n + j];
                }
                b[offsetB + i*n + j] = sum/L[offsetL + i*strideL + i];
            }
        }
    }

    /**
     * <p>
     * Solves for non-singular lower triangular matrices using forward substitution.
     * <br>
     * B<sup>T</sup> = L<sup>-1</sup>B<sup>T</sup><br>
     * <br>
     * where B is a (n by m) matrix, L is a lower triangular (m by m) matrix.
     * </p>
     *
     * @param L An m by m non-singular lower triangular matrix. Not modified.
     * @param b An n by m matrix. Modified.
     * @param m size of the L matrix
     * @param n number of columns in the B matrix.
     * @param offsetL initial index in L where the matrix starts
     * @param offsetB initial index in B where the matrix starts
     */
    public static void solveLTransB( double[] L, double[] b,
                                     int m, int n,
                                     int strideL, int offsetL, int offsetB ) {
//        for( int j = 0; j < n; j++ ) {
//            for( int i = 0; i < m; i++ ) {
//                double sum = b[offsetB + j*m+i];
//                for( int k=0; k<i; k++ ) {
//                    sum -= L[offsetL + i*m+k]* b[offsetB + j*m+k];
//                }
//                b[offsetB + j*m+i] = sum / L[offsetL + i*m+i];
//            }
//        }
        for (int j = 0; j < n; j++) {
            for (int i = 0; i < m; i++) {
                double sum = b[offsetB + j*m + i];
                int l = offsetL + i*strideL;
                int bb = offsetB + j*m;
                int endL = l + i;
                while (l != endL) {
//                for( int k=0; k<i; k++ ) {
                    sum -= L[l++]*b[bb++];
                }
                b[offsetB + j*m + i] = sum/L[offsetL + i*strideL + i];
            }
        }
    }

    /**
     * <p>
     * Solves for non-singular upper triangular matrices using backwards substitution.
     * <br>
     * B = U<sup>-1</sup>B<br>
     * <br>
     * where B (m by n) is a matrix, U is a (m by m ) upper triangular matrix.<br>
     * </p>
     *
     * @param U An m by m non-singular upper triangular matrix. Not modified.
     * @param b An m by n matrix. Modified.
     * @param m size of the L matrix
     * @param n number of columns in the B matrix.
     * @param offsetU initial index in L where the matrix starts
     * @param offsetB initial index in B where the matrix starts
     */
    public static void solveU( double[] U, double[] b,
                               int m, int n,
                               int strideU, int offsetU, int offsetB ) {
        for (int j = 0; j < n; j++) {
            for (int i = m - 1; i >= 0; i--) {
                double sum = b[offsetB + i*n + j];
                for (int k = i + 1; k < m; k++) {
                    sum -= U[offsetU + i*strideU + k]*b[offsetB + k*n + j];
                }
                b[offsetB + i*n + j] = sum/U[offsetU + i*strideU + i];
            }
        }
    }

    /**
     * <p>
     * Solves for non-singular upper triangular matrices using forward substitution.
     * <br>
     * B = U<sup>-T</sup>B<br>
     * <br>
     * where B (m by n) is a matrix, U is a (m by m ) upper triangular matrix.<br>
     * </p>
     *
     * @param U An m by m non-singular upper triangular matrix. Not modified.
     * @param b An m by n matrix. Modified.
     * @param m size of the L matrix
     * @param n number of columns in the B matrix.
     * @param offsetU initial index in L where the matrix starts
     * @param offsetB initial index in B where the matrix starts
     */
    public static void solveTransU( double[] U, double[] b,
                                    int m, int n,
                                    int strideU, int offsetU, int offsetB ) {
        for (int j = 0; j < n; j++) {
            for (int i = 0; i < m; i++) {
                double sum = b[offsetB + i*n + j];
                for (int k = 0; k < i; k++) {
                    sum -= U[offsetU + k*strideU + i]*b[offsetB + k*n + j];
                }
                b[offsetB + i*n + j] = sum/U[offsetU + i*strideU + i];
            }
        }
    }
}
