/*
 * matrix_utils.cpp
 *
 *  Created on: Oct 28, 2015
 *      Author: marko
 */

#include <linalg/matrix_utils.h>
#include <linalg/invalid_matrix_operation_exception.h>
#include <cmath>
#include <iostream>

using namespace std;
namespace linalg {

void MatrixUtils::fwdSupstitution(Matrix& A, Matrix& b) {
    if(!A.isSquare()) {
        throw InvalidMatrixOperationException("Matrix A is not square");
    }
    if(A.cols() != b.rows() || b.cols() != 1) {
        throw InvalidMatrixOperationException(
                "Matrix B should be row vector (A.cols x 1)");
    }

    int N = A.rows();
    for (int i = 0; i < N; ++i) {
        for (int j = i + 1; j < N; ++j) {
            b[j][0] -= A[j][i] * b[i][0];
        }
    }

}

void MatrixUtils::bkwdSupstitution(Matrix& A, Matrix& b) {
    if(!A.isSquare()) {
        throw InvalidMatrixOperationException("Matrix A is not square");
    }
    if(A.cols() != b.rows() || b.cols() != 1) {
        throw InvalidMatrixOperationException(
                "Matrix B should be row vector (A.cols x 1)");
    }

    int N = A.rows();
    for (int i = N - 1; i >= 0; --i) {
        if(fabs(A[i][i]) < EPS) {
            throw InvalidMatrixOperationException(
                    "Could not solve. Division by zero");
        }
        b[i][0] /= A[i][i];

        for (int j = 0; j < i; ++j) {
            b[j][0] -= A[j][i] * b[i][0];
        }
    }

}

void MatrixUtils::decomposeLU(Matrix& A) {
    if(!A.isSquare()) {
        throw InvalidMatrixOperationException("Matrix A is not square");
    }
    int N = A.rows();

    for (int i = 0; i < N; ++i) {
        for (int j = i + 1; j < N; ++j) {
            if(fabs(A[i][i]) < EPS) {
                throw InvalidMatrixOperationException(
                        "Could not decompose. 0 on main diagonal");
            }
            A[j][i] /= A[i][i];
            for (int k = i + 1; k < N; ++k) {
                A[j][k] -= A[j][i] * A[i][k];
            }
        }
    }

}

Matrix MatrixUtils::decomposeLUP(Matrix& A) {
    if(!A.isSquare()) {
        throw InvalidMatrixOperationException("Matrix A is not square");
    }
    int N = A.rows();
    Matrix p(N, N);
    p.eye();

    for (int i = 0; i < N; ++i) {
        int pivot = i;
        for (int j = i + 1; j < N; ++j) {
            if(fabs(A[j][i]) > fabs(A[pivot][i])) {
                pivot = j;
            }
        }


        p.swapRows(i, pivot);
        A.swapRows(i, pivot);

        //cout << A << endl <<endl;
        //cout << A[i][i] << endl;

        for (int j = i + 1; j < N; ++j) {
            A[j][i] /= A[i][i];
            for (int k = i + 1; k < N; ++k) {

                //cout << A[j][k] << "  " << A[j][i] << " * " << A[i][k]<<endl;
                A[j][k] -= A[j][i] * A[i][k];
            }
        }

    }

    return p;
}

void MatrixUtils::solveSystem(Matrix&A, Matrix& B) {
    Matrix permutation = MatrixUtils::decomposeLUP(A);
    B = permutation * B;

    MatrixUtils::fwdSupstitution(A, B);
    MatrixUtils::bkwdSupstitution(A, B);
}

std::vector<double> MatrixUtils::toVector(Matrix& m) {
    std::vector<double> x;
    if(m.cols() != 1) throw InvalidMatrixOperationException("Cannot convert to row vector");
    for(int i = 0; i < m.rows(); ++i) {
        x.emplace_back(m[i][0]);
    }
    return x;
}

} /* namespace linalg */