#include "aghMatrix.h"
#include <cmath>

// Parameter Constructor
template <typename T>
AGHMatrix<T>::AGHMatrix(const std::vector<std::vector<T>>& mat) {
    matrix.resize(mat.size());
    for (unsigned i = 0; i < mat.size(); i++) {
        matrix[i].resize(mat[i].size());
        for (unsigned j = 0; j < mat[i].size(); j++) {
            matrix[i][j] = mat[i][j];
        }
    }
    rows = matrix.size();
    cols = matrix[0].size();
}

// Parameter Constructor
template <typename T>
AGHMatrix<T>::AGHMatrix(unsigned _rows, unsigned _cols, const T& _initial) {
    matrix.resize(_rows);
    for (unsigned i = 0; i < matrix.size(); i++) {
        matrix[i].resize(_cols, _initial);
    }
    rows = _rows;
    cols = _cols;
}

// Copy Constructor
template <typename T>
AGHMatrix<T>::AGHMatrix(const AGHMatrix<T>& rhs) {
    matrix = rhs.matrix;
    rows = rhs.get_rows();
    cols = rhs.get_cols();
}

// Get the number of rows of the matrix
template <typename T>
unsigned AGHMatrix<T>::get_rows() const {
    return this->rows;
}

// Get the number of columns of the matrix
template <typename T>
unsigned AGHMatrix<T>::get_cols() const {
    return this->cols;
}

// Assignment Operator
template <typename T>
AGHMatrix<T>& AGHMatrix<T>::operator=(const AGHMatrix<T>& rhs) {
    if (&rhs == this) return *this;

    unsigned new_rows = rhs.get_rows();
    unsigned new_cols = rhs.get_cols();

    matrix.resize(new_rows);
    for (unsigned i = 0; i < matrix.size(); i++) {
        matrix[i].resize(new_cols);
    }

    for (unsigned i = 0; i < new_rows; i++) {
        for (unsigned j = 0; j < new_cols; j++) {
            matrix[i][j] = rhs(i, j);
        }
    }
    rows = new_rows;
    cols = new_cols;

    return *this;
}

// Access the individual elements
template <typename T>
T& AGHMatrix<T>::operator()(const unsigned& row, const unsigned& col) {
    return this->matrix[row][col];
}

// Access the individual elements (const)
template <typename T>
const T& AGHMatrix<T>::operator()(const unsigned& row,
                                  const unsigned& col) const {
    return this->matrix[row][col];
}

// Addition of two matrices
template <typename T>
AGHMatrix<T> AGHMatrix<T>::operator+(const AGHMatrix<T>& rhs) {
    // Task 1 - implement addition of two matrices
    AGHMatrix<T> new_matrix = *this;

    for (int y = 0; y < this->rows; y++) {
        for (int x = 0; x < this->cols; x++) {
            new_matrix.matrix[y][x] += rhs(y, x);
        }
    }
    return new_matrix;
}

template <typename T>
AGHMatrix<T> AGHMatrix<T>::operator-(const AGHMatrix<T>& rhs) {
    // Task 1 - implement addition of two matrices
    AGHMatrix<T> new_matrix = *this;

    for (int y = 0; y < this->rows; y++) {
        for (int x = 0; x < this->cols; x++) {
            new_matrix.matrix[y][x] -= rhs(y, x);
        }
    }
    return new_matrix;
}

// Left multiplication of this matrix and another
template <typename T>
AGHMatrix<T> AGHMatrix<T>::operator*(const AGHMatrix<T>& rhs) {
    // Task 1 - implement multiplication of two matrices

    AGHMatrix<T>* new_matrix = new AGHMatrix(this->rows, rhs.cols, 0);

    for (int x = 0; x < rhs.cols; x++) {
        for (int y = 0; y < this->rows; y++) {
            for (int i = 0; i < this->rows; i++) {
                new_matrix->matrix[y][x] +=
                    this->matrix[y][i] * rhs.matrix[i][x];
            }
        }
    }

    return *new_matrix;
}

// Printing matrix
template <typename T>
std::ostream& operator<<(std::ostream& stream, const AGHMatrix<T>& matrix) {
    for (int i = 0; i < matrix.get_rows(); i++) {
        for (int j = 0; j < matrix.get_cols(); j++) {
            stream << matrix(i, j) << ", ";
        }
        stream << std::endl;
    }
    stream << std::endl;

    return stream;
}

template <typename T>
bool AGHMatrix<T>::is_symmetric() const {
    if (this->cols != this->rows) return false;

    for (int y = 0; y < this->rows; y++) {
        for (int x = 0; x < this->cols; x++) {
            if (this->matrix[y][x] != this->matrix[x][y]) return false;
        }
    }

    return true;
}

template <typename T>
AGHMatrix<T> AGHMatrix<T>::without_row_and_column(int row, int col) {
    AGHMatrix<T> new_matrix = *this;

    new_matrix.rows--;
    new_matrix.cols--;

    new_matrix.matrix.erase(new_matrix.matrix.begin() + row);

    for (int y = 0; y < new_matrix.rows; y++) {
        new_matrix.matrix[y].erase(new_matrix.matrix[y].begin() + col);
    }

    return new_matrix;
}

template <typename T>
T AGHMatrix<T>::determinant() {
    if (this->cols == 1) return this->matrix[0][0];

    T retval = 0;
    int coeff = 1;

    for (int x = 0; x < this->cols; x++) {
        T det = this->without_row_and_column(0, x).determinant();
        retval += coeff * this->matrix[0][x] * det;
        coeff *= -1;
    }

    return retval;
}

template <typename T>
AGHMatrix<T> AGHMatrix<T>::transpose() {
    AGHMatrix<T> retval = AGHMatrix(this->cols, this->rows, 0);

    for (int y = 0; y < this->rows; y++) {
        for (int x = 0; x < this->cols; x++) {
            retval.matrix[x][y] = this->matrix[y][x];
        }
    }

    return retval;
}

template <typename T>
std::pair<AGHMatrix<T>, AGHMatrix<T>> AGHMatrix<T>::LU() {
    AGHMatrix<T> U = *this;
    AGHMatrix<T> L = AGHMatrix<T>(this->rows, this->cols, 0);
    for (int y = 0; y < this->rows; y++) {
        L(y, y) = 1;
    }

    for (int x = 0; x < this->cols; x++) {
        for (int i = x + 1; i < this->cols; i++) {
            T l = -U(i, x) / U(x, x);

            for (int j = x; j < this->cols; j++) {
                U(i, j) += l * U(x, j);
            }

            L(i, x) = -l;
        }
    }

    return std::make_pair(L, U);
}

template <typename T>
AGHMatrix<T> AGHMatrix<T>::cholesky() {
    AGHMatrix<T> L = AGHMatrix<T>(this->rows, this->cols, 0);

    for (int y = 0; y < this->rows; y++) {
        for (int x = 0; x <= y; x++) {
            T acc = this->matrix[y][x];
            if (y == x) {
                for (int i = 0; i < x; i++) {
                    acc -= pow(L(x, i), 2);
                }
                L(x, x) = sqrt(acc);
            } else {
                for (int i = 0; i < x; i++) {
                    acc -= L(x, i) * L(y, i);
                }
                L(y, x) = acc / L(x, x);
            }
        }
    }

    return L;
}

template <typename T>
AGHMatrix<T> AGHMatrix<T>::gauss(AGHMatrix<T> b) {
    AGHMatrix<T> A = *this;

    for (int x = 0; x < A.cols; x++) {
        for (int y = x; y < A.rows; y++) {
            if (A.matrix[y][x] == 0) continue;

            T f = A.matrix[y][x];
            for (int i = x; i < A.cols; i++) {
                A.matrix[y][i] /= f;
            }
            b.matrix[y][0] /= f;

            for (int i = y + 1; i < A.rows; i++) {
                T c = A.matrix[i][x];
                for (int j = x; j < A.cols; j++) {
                    A.matrix[i][j] -= c * A.matrix[y][j];
                }
                b.matrix[i][0] -= c * b.matrix[y][0];
            }
        }
    }

    for (int x = A.cols - 1; x >= 0; x--) {
        for (int y = x; y >= 0; y--) {
            if (A.matrix[y][x] == 0) continue;

            for (int i = y - 1; i >= 0; i--) {
                b.matrix[i][0] -= A.matrix[i][x] * b.matrix[y][0];
                A.matrix[i][x] = 0;
            }
        }
    }

    return b;
}

template <typename T>
T AGHMatrix<T>::length() {
    T retval = 0;
    for (int y = 0; y < this->rows; y++) {
        retval += pow(this->matrix[y][0], 2);
    }
    return retval;
}

template <typename T>
AGHMatrix<T> AGHMatrix<T>::jacobi(AGHMatrix<T> b) {
    AGHMatrix<T> A = *this;

    AGHMatrix<T> x = AGHMatrix<T>(A.rows, 1, 1);
    AGHMatrix<T> prev_x = AGHMatrix<T>(A.rows, 1, 0);

    while (std::abs((x - prev_x).length()) > 0.00001) {
        prev_x = x;

        for (int y = 0; y < A.rows; y++) {
            x(y, 0) = b(y, 0);
            for (int j = 0; j < A.cols; j++) {
                if (j == y) continue;
                x(y, 0) -= A(y, j) * prev_x(j, 0);
            }
            x(y, 0) /= A(y, y);
        }
    }

    return x;
}