//
//  LinAlg.cpp
//
//  Created by Marc Melikyan on 1/8/21.
//

#include "LinAlg.hpp"
#include "Stat/Stat.hpp"
#include <iostream>
#include <map>
#include <cmath>

namespace MLPP{

    std::vector<std::vector<double>> LinAlg::addition(std::vector<std::vector<double>> A, std::vector<std::vector<double>> B){
        std::vector<std::vector<double>> C;
        C.resize(A.size());
        for(int i = 0; i < C.size(); i++){
            C[i].resize(A[0].size());
        }
        
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[0].size(); j++){
                C[i][j] = A[i][j] + B[i][j];
            }
        }
        return C;
    }

    std::vector<std::vector<double>> LinAlg::subtraction(std::vector<std::vector<double>> A, std::vector<std::vector<double>> B){
        std::vector<std::vector<double>> C;
        C.resize(A.size());
        for(int i = 0; i < C.size(); i++){
            C[i].resize(A[0].size());
        }

        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[0].size(); j++){
                C[i][j] = A[i][j] - B[i][j];
            }
        }
        return C;
    }

    std::vector<std::vector<double>> LinAlg::matmult(std::vector<std::vector<double>> A, std::vector<std::vector<double>> B){
        std::vector<std::vector<double>> C;
        C.resize(A.size());
        for(int i = 0; i < C.size(); i++){
            C[i].resize(B[0].size());
        }
        
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < B[0].size(); j++){
                for(int k = 0; k < B.size(); k++){
                    C[i][j] += A[i][k] * B[k][j];
                }
            }
        }
        return C;
    }

    std::vector<std::vector<double>> LinAlg::hadamard_product(std::vector<std::vector<double>> A, std::vector<std::vector<double>> B){
        std::vector<std::vector<double>> C;
        C.resize(A.size());
        for(int i = 0; i < C.size(); i++){
            C[i].resize(A[0].size());
        }
        
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[0].size(); j++){
                C[i][j] = A[i][j] * B[i][j];
            }
        }
        return C;
    }

    std::vector<std::vector<double>> LinAlg::elementWiseDivision(std::vector<std::vector<double>> A, std::vector<std::vector<double>> B){
        std::vector<std::vector<double>> C;
        C.resize(A.size());
        for(int i = 0; i < C.size(); i++){
            C[i].resize(A[0].size());
        }
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[i].size(); j++){
                C[i][j] = A[i][j] / B[i][j];
            }
        }
        return C;
    }

    std::vector<std::vector<double>> LinAlg::transpose(std::vector<std::vector<double>> A){
        std::vector<std::vector<double>> AT;
        AT.resize(A[0].size());
        for(int i = 0; i < AT.size(); i++){
            AT[i].resize(A.size());
        }
        
        for(int i = 0; i < A[0].size(); i++){
            for(int j = 0; j < A.size(); j++){
                AT[i][j] = A[j][i];
            }
        }
        return AT;
    }

    std::vector<std::vector<double>> LinAlg::scalarMultiply(double scalar, std::vector<std::vector<double>> A){
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[i].size(); j++){
                A[i][j] *= scalar;
            }
        }
        return A;
    }

    std::vector<std::vector<double>> LinAlg::scalarAdd(double scalar, std::vector<std::vector<double>> A){
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[i].size(); j++){
                A[i][j] += scalar;
            }
        }
        return A;
    }

    std::vector<std::vector<double>> LinAlg::log(std::vector<std::vector<double>> A){
        std::vector<std::vector<double>> B;
        B.resize(A.size());
        for(int i = 0; i < B.size(); i++){
            B[i].resize(A[0].size());
        }
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[i].size(); j++){
                B[i][j] = std::log(A[i][j]);
            }
        }
        return B;
    }

    std::vector<std::vector<double>> LinAlg::log10(std::vector<std::vector<double>> A){
        std::vector<std::vector<double>> B;
        B.resize(A.size());
        for(int i = 0; i < B.size(); i++){
            B[i].resize(A[0].size());
        }
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[i].size(); j++){
                B[i][j] = std::log10(A[i][j]);
            }
        }
        return B;
    }

    std::vector<std::vector<double>> LinAlg::exp(std::vector<std::vector<double>> A){
        std::vector<std::vector<double>> B;
        B.resize(A.size());
        for(int i = 0; i < B.size(); i++){
            B[i].resize(A[0].size());
        }
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[i].size(); j++){
                B[i][j] = std::exp(A[i][j]);
            }
        }
        return B;
    }

    std::vector<std::vector<double>> LinAlg::erf(std::vector<std::vector<double>> A){
        std::vector<std::vector<double>> B;
        B.resize(A.size());
        for(int i = 0; i < B.size(); i++){
            B[i].resize(A[0].size());
        }
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[i].size(); j++){
                B[i][j] = std::erf(A[i][j]);
            }
        }
        return B;
    }

    std::vector<std::vector<double>> LinAlg::exponentiate(std::vector<std::vector<double>> A, double p){
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[i].size(); j++){
                A[i][j] = pow(A[i][j], p); 
            }
        }
        return A; 
    }

    double LinAlg::det(std::vector<std::vector<double>> A, int d){

        double deter = 0;
        std::vector<std::vector<double>> B;
        B.resize(d);
        for(int i = 0; i < d; i++){
            B[i].resize(d);
        }

        /* This is the base case in which the input is a 2x2 square matrix.
        Recursion is performed unless and until we reach this base case,
        such that we recieve a scalar as the result. */
        if(d == 2){
            return A[0][0] * A[1][1] - A[0][1] * A[1][0];
        }

        else{
            for(int i = 0; i < d; i++){
                int sub_i = 0;
                for(int j = 1; j < d; j++){
                    int sub_j = 0;
                    for(int k = 0; k < d; k++){
                        if(k == i){
                            continue;
                        }
                        B[sub_i][sub_j] = A[j][k];
                        sub_j++;
                    }
                    sub_i++;
                }
                deter += pow(-1, i) * A[0][i] * det(B, d-1);
            }
        }
        return deter;
    }

    std::vector<std::vector<double>> LinAlg::cofactor(std::vector<std::vector<double>> A, int n, int i, int j){
        std::vector<std::vector<double>> cof;
        cof.resize(A.size());
        for(int i = 0; i < cof.size(); i++){
          cof[i].resize(A.size());
        }
        int sub_i = 0, sub_j = 0;
      
        for (int row = 0; row < n; row++){
            for (int col = 0; col < n; col++){
                if (row != i && col != j) {
                    cof[sub_i][sub_j++] = A[row][col];
      
                    if (sub_j == n - 1){
                        sub_j = 0;
                        sub_i++;
                    }
                }
            }
        }
        return cof;
    }

    std::vector<std::vector<double>> LinAlg::adjoint(std::vector<std::vector<double>> A){

        //Resizing the initial adjoint matrix
        std::vector<std::vector<double>> adj;
        adj.resize(A.size());
        for(int i = 0; i < adj.size(); i++){
            adj[i].resize(A.size());
        }

        // Checking for the case where the given N x N matrix is a scalar
        if(A.size() == 1){
            adj[0][0] = 1;
            return adj;
        }
        
        if(A.size() == 2){
            adj[0][0] = A[1][1];
            adj[1][1] = A[0][0];
            
            adj[0][1] = -A[0][1];
            adj[1][0] = -A[1][0];
            return adj;
        }

      for(int i = 0; i < A.size(); i++){
        for(int j = 0; j < A.size(); j++){
          std::vector<std::vector<double>> cof = cofactor(A, int(A.size()), i, j);
          // 1 if even, -1 if odd
          int sign = (i + j) % 2 == 0 ? 1 : -1;
          adj[j][i] = sign * det(cof, int(A.size()) - 1);
        }
      }
      return adj;
    }

    // The inverse can be computed as (1 / determinant(A)) * adjoint(A)
    std::vector<std::vector<double>> LinAlg::inverse(std::vector<std::vector<double>> A){
      return scalarMultiply(1/det(A, int(A.size())), adjoint(A));
    }
    
    // This is simply the Moore-Penrose least squares approximation of the inverse. 
    std::vector<std::vector<double>> LinAlg::pinverse(std::vector<std::vector<double>> A){
        return matmult(inverse(matmult(transpose(A), A)), transpose(A));
    }

    std::vector<std::vector<double>> LinAlg::zeromat(int n, int m){
        std::vector<std::vector<double>> zeromat;
        zeromat.resize(n);
        for(int i = 0; i < zeromat.size(); i++){
            zeromat[i].resize(m);
        }
        return zeromat; 
    }

    std::vector<std::vector<double>> LinAlg::onemat(int n, int m){
        return full(n, m, 1);
    }

    std::vector<std::vector<double>> LinAlg::full(int n, int m, int k){
        std::vector<std::vector<double>> full;
        full.resize(n);
        for(int i = 0; i < full.size(); i++){
            full[i].resize(m);
        }
        for(int i = 0; i < full.size(); i++){
            for(int j = 0; j < full[i].size(); j++){
                full[i][j] = k; 
            }
        }
        return full; 
    }

    std::vector<std::vector<double>> LinAlg::round(std::vector<std::vector<double>> A){
        std::vector<std::vector<double>> B;
        B.resize(A.size());
        for(int i = 0; i < B.size(); i++){
            B[i].resize(A[0].size());
        }
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[i].size(); j++){
                B[i][j] = std::round(A[i][j]);
            }
        }
        return B;
    }

    std::vector<std::vector<double>> LinAlg::identity(double d){
        std::vector<std::vector<double>> identityMat; 
        identityMat.resize(d);
        for(int i = 0; i < identityMat.size(); i++){
            identityMat[i].resize(d);
        }
        for(int i = 0; i < identityMat.size(); i++){
            for(int j = 0; j < identityMat.size(); j++){
                if(i == j){
                    identityMat[i][j] = 1;
                }
                else { identityMat[i][j] = 0; }
            }
        }
        return identityMat;
    }

    std::vector<std::vector<double>> LinAlg::cov(std::vector<std::vector<double>> A){
        Stat stat;
        std::vector<std::vector<double>> covMat;
        covMat.resize(A.size());
        for(int i = 0; i < covMat.size(); i++){
            covMat[i].resize(A.size());
        }
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A.size(); j++){
                covMat[i][j] = stat.covariance(A[i], A[j]);
            }
        }
        return covMat;
    }

    std::tuple<std::vector<std::vector<double>>, std::vector<std::vector<double>>> LinAlg::eig(std::vector<std::vector<double>> A){
        /*
        A (the entered parameter) in most use cases will be X'X, XX', etc. and must be symmetric.
        That simply means that 1) X' = X and 2) X is a square matrix. This function that computes the 
        eigenvalues of a matrix is utilizing Jacobi's method. 
        */

        double diagonal = true; // Perform the iterative Jacobi algorithm unless and until we reach a diagonal matrix which yields us the eigenvals. 
        
        std::map<int, int> val_to_vec; 
        std::vector<std::vector<double>> a_new;
        std::vector<std::vector<double>> eigenvectors = identity(A.size());
        do{
            double a_ij = A[0][1];
            double sub_i = 0; 
            double sub_j = 1;
            for(int i = 0; i < A.size(); i++){
                for(int j = 0; j < A[i].size(); j++){
                    if(i != j && abs(A[i][j]) > a_ij){
                        a_ij = A[i][j];
                        sub_i = i; 
                        sub_j = j;
                    }
                    else if(i != j && abs(A[i][j]) == a_ij){
                        if(i < sub_i){
                            a_ij = A[i][j];
                            sub_i = i; 
                            sub_j = j;
                        }
                    }
                }
            }

            double a_ii = A[sub_i][sub_i];
            double a_jj = A[sub_j][sub_j]; 
            double a_ji = A[sub_j][sub_i]; 
            double theta; 

            if(a_ii == a_jj) {
                theta = M_PI / 4; 
            }
            else{
                theta = 0.5 * atan(2 * a_ij / (a_ii - a_jj));
            }

            std::vector<std::vector<double>> P = identity(A.size());
            P[sub_i][sub_j] = -sin(theta);
            P[sub_i][sub_i] = cos(theta);
            P[sub_j][sub_j] = cos(theta);
            P[sub_j][sub_i] = sin(theta);

            a_new = matmult(matmult(inverse(P), A), P);

            for(int i = 0; i < a_new.size(); i++){
                for(int j = 0; j < a_new[i].size(); j++){
                    if(i != j && std::round(a_new[i][j]) == 0){
                        a_new[i][j] = 0;
                    }
                }
            }

            bool non_zero = false;
            for(int i = 0; i < a_new.size(); i++){
                for(int j = 0; j < a_new[i].size(); j++){
                    if(i != j && std::round(a_new[i][j]) != 0){
                        non_zero = true;
                    }
                }
            }   

            if(non_zero) { 
                diagonal = false;
            }
            else{
                diagonal = true;
            }

            if(a_new == A){
                diagonal = true; 
                for(int i = 0; i < a_new.size(); i++){
                    for(int j = 0; j < a_new[i].size(); j++){
                        if(i != j){
                            a_new[i][j] = 0;
                        }
                    }
                }   
            }
            
            eigenvectors = matmult(eigenvectors, P);
            A = a_new;

        } while(!diagonal);

        std::vector<std::vector<double>> a_new_prior = a_new;
        
        // Bubble Sort
        for(int i = 0; i < a_new.size() - 1; i++){
            for(int j = 0; j < a_new.size() - 1 - i; j++){
                if(a_new[j][j] < a_new[j + 1][j + 1]){
                    double temp = a_new[j + 1][j + 1];
                    a_new[j + 1][j + 1] = a_new[j][j];
                    a_new[j][j] = temp;
                }
            }
        }


        for(int i = 0; i < a_new.size(); i++){
            for(int j = 0; j < a_new.size(); j++){
                if(a_new[i][i] == a_new_prior[j][j]){
                    val_to_vec[i] = j;
                }
            }
        }

        std::vector<std::vector<double>> eigen_temp = eigenvectors;
        for(int i = 0; i < eigenvectors.size(); i++){
            for(int j = 0; j < eigenvectors[i].size(); j++){
                eigenvectors[i][j] = eigen_temp[i][val_to_vec[j]];  
            }
        }
        return {eigenvectors, a_new};

    }

    std::tuple<std::vector<std::vector<double>>, std::vector<std::vector<double>>, std::vector<std::vector<double>>> LinAlg::SVD(std::vector<std::vector<double>> A){
        auto [left_eigenvecs, eigenvals] = eig(matmult(A, transpose(A)));
        auto [right_eigenvecs, right_eigenvals] = eig(matmult(transpose(A), A));

        std::vector<std::vector<double>> singularvals = exponentiate(eigenvals, 0.5);
        std::vector<std::vector<double>> sigma = zeromat(A.size(), A[0].size());
         for(int i = 0; i < singularvals.size(); i++){
            for(int j = 0; j < singularvals[i].size(); j++){
                sigma[i][j] = singularvals[i][j];
            }
        }
        return {left_eigenvecs, sigma, right_eigenvecs};
    }

    double LinAlg::sum_elements(std::vector<std::vector<double>> A){
        double sum = 0;
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[i].size(); j++){
                sum += A[i][j];
            }
        }
        return sum;
    }

    std::vector<double> LinAlg::flatten(std::vector<std::vector<double>> A){
        std::vector<double> a; 
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[i].size(); j++){
                a.push_back(A[i][j]);
            }
        }
        return a;
    }

    void LinAlg::printMatrix(std::vector<std::vector<double>> A){
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[i].size(); j++){
                std::cout << A[i][j] << " ";
            }
            std::cout << std::endl;
        }
    }

    std::vector<std::vector<double>> LinAlg::vecmult(std::vector<double> a, std::vector<double> b){
        std::vector<std::vector<double>> C;
        C.resize(a.size());
        for(int i = 0; i < C.size(); i++){
            C[i] = scalarMultiply(a[i], b);
        }
        return C;
    }

    std::vector<double> LinAlg::hadamard_product(std::vector<double> a, std::vector<double> b){
        std::vector<double> c;
        c.resize(a.size());
        
        for(int i = 0; i < a.size(); i++){
            c[i] = a[i] * b[i];
        }
        
        return c;
    }

    std::vector<double> LinAlg::elementWiseDivision(std::vector<double> a, std::vector<double> b){
        std::vector<double> c;
        c.resize(a.size());

        for(int i = 0; i < a.size(); i++){
            c[i] = a[i] / b[i];
        }
        return c;
    }

    std::vector<double> LinAlg::scalarMultiply(double scalar, std::vector<double> a){
        for(int i = 0; i < a.size(); i++){
            a[i] *= scalar;
        }
        return a;
    }

    std::vector<double> LinAlg::scalarAdd(double scalar, std::vector<double> a){
        for(int i = 0; i < a.size(); i++){
            a[i] += scalar;
        }
        return a;
    }

    std::vector<double> LinAlg::addition(std::vector<double> a, std::vector<double> b){
        std::vector<double> c;
        c.resize(a.size());
        for(int i = 0; i < a.size(); i++){
            c[i] = a[i] + b[i];
        }
        return c;
    }

    std::vector<double> LinAlg::subtraction(std::vector<double> a, std::vector<double> b){
        std::vector<double> c;
        c.resize(a.size());
        for(int i = 0; i < a.size(); i++){
            c[i] = a[i] - b[i];
        }
        return c;
    }

    std::vector<double> LinAlg::subtractMatrixRows(std::vector<double> a, std::vector<std::vector<double>> B){
        for(int i = 0; i < B.size(); i++){
            a = subtraction(a, B[i]);
        }
        return a; 
    }

    std::vector<double> LinAlg::log(std::vector<double> a){
        std::vector<double> b; 
        b.resize(a.size());
        for(int i = 0; i < a.size(); i++){
            b[i] = std::log(a[i]);
        }
        return b; 
    }

    std::vector<double> LinAlg::log10(std::vector<double> a){
        std::vector<double> b; 
        b.resize(a.size());
        for(int i = 0; i < a.size(); i++){
            b[i] = std::log10(a[i]);
        }
        return b; 
    }

    std::vector<double> LinAlg::exp(std::vector<double> a){
        std::vector<double> b;
        b.resize(a.size());
        for(int i = 0; i < a.size(); i++){
            b[i] = std::exp(a[i]);
        }
        return b;
    }

    std::vector<double> LinAlg::erf(std::vector<double> a){
        std::vector<double> b;
        b.resize(a.size());
        for(int i = 0; i < a.size(); i++){
            b[i] = std::erf(a[i]);
        }
        return b;
    }

    std::vector<double> LinAlg::exponentiate(std::vector<double> a, double p){
        std::vector<double> b; 
        b.resize(a.size());
        for(int i = 0; i < b.size(); i++){
            b[i] = pow(a[i], p); 
        }
        return b;
    }

    double LinAlg::dot(std::vector<double> a, std::vector<double> b){
        double c = 0;
        for(int i = 0; i < a.size(); i++){
            c += a[i] * b[i];
        }
        return c;
    }

    std::vector<double> LinAlg::zerovec(int n){
        std::vector<double> zerovec; 
        zerovec.resize(n);
        return zerovec;
    }

    std::vector<double> LinAlg::onevec(int n){
        return full(n, 1);
    }

    std::vector<double> LinAlg::full(int n, int k){
        std::vector<double> full; 
        full.resize(n);
        for(int i = 0; i < full.size(); i++){
            full[i] = k;
        }
        return full;
    }

    double LinAlg::max(std::vector<double> a){
        int max = a[0];
        for(int i = 0; i < a.size(); i++){
            if(a[i] > max){
                max = a[i];
            }
        }
        return max; 
    }

    double LinAlg::min(std::vector<double> a){
        int min = a[0];
        for(int i = 0; i < a.size(); i++){
            if(a[i] < min){
                min = a[i];
            }
        }
        return min; 
    }

    std::vector<double> LinAlg::round(std::vector<double> a){
        std::vector<double> b;
        b.resize(a.size());
        for(int i = 0; i < a.size(); i++){
            b[i] = std::round(a[i]);
        }
        return b;
    }

    double LinAlg::norm_sq(std::vector<double> a){
        double n_sq = 0;
        for(int i = 0; i < a.size(); i++){
            n_sq += a[i] * a[i];
        }
        return n_sq;
    }

    double LinAlg::sum_elements(std::vector<double> a){
        double sum = 0;
        for(int i = 0; i < a.size(); i++){
            sum += a[i];
        }
        return sum;
    }

    double LinAlg::cosineSimilarity(std::vector<double> a, std::vector<double> b){
        return dot(a, b) / (sqrt(norm_sq(a)) * sqrt(norm_sq(b)));
    }

    void LinAlg::printVector(std::vector<double> a){
        for(int i = 0; i < a.size(); i++){
            std::cout << a[i] << " ";
        }
        std::cout << std::endl;
    }

    std::vector<std::vector<double>> LinAlg::mat_vec_add(std::vector<std::vector<double>> A, std::vector<double> b){
        for(int i = 0; i < A.size(); i++){
            for(int j = 0; j < A[i].size(); j++){
                A[i][j] += b[j];
            }
        }
        return A;
    }

    std::vector<double> LinAlg::mat_vec_mult(std::vector<std::vector<double>> A, std::vector<double> b){
        std::vector<double> c;
        c.resize(A.size());
            
        for(int i = 0; i < A.size(); i++){
            for(int k = 0; k < b.size(); k++){
                c[i] += A[i][k] * b[k];
            }
        }
        return c;
    }

    std::vector<double> LinAlg::flatten(std::vector<std::vector<std::vector<double>>> A){
        std::vector<double> c;
        for(int i = 0; i < A.size(); i++){
            std::vector<double> flattenedVec = flatten(A[i]);
            c.insert(c.end(), flattenedVec.begin(), flattenedVec.end());
        }
        return c;
    }

    void LinAlg::printTensor(std::vector<std::vector<std::vector<double>>> A){
        for(int i = 0; i < A.size(); i++){
            printMatrix(A[i]);
            if(i != A.size() - 1) { std::cout << std::endl; }
        }
    }
}