#ifndef _inverse_gj_n_
#define _inverse_gj_n_


#include "./utils/vector.h"
#include "./utils/matrix.h"

#include "./numerics/initializers/eye.h"

#include <omp.h>

namespace numerics{
	
	template <typename T>
   	void inverse_gj(utils::Matrix<T>& A){
		//utils::Matrix<T> B(A.rows(),A.cols(), T{0});
		utils::Matrix<T> B;
		B = eye_omp_auto<T>(A.rows());


		uint64_t icol{0}, irow{0}, rows{A.rows()}, cols{A.cols()};
		double big, dum, pivinv, temp;
		utils::Vi indxc(rows,0), indxr(rows,0), ipiv(rows,0);

		//for (uint64_t j = 0; j < N; ++j){ ipiv[j] = 0;}

		for (uint64_t i = 0; i < rows; i++){
			big = 0.0;
			for (uint64_t j = 0; j < rows; j++){
				if (ipiv[j] != 1){
					for (uint64_t k = 0; k < rows; k++){
						if (ipiv[k] == 0){
							if (abs(A(j,k)) >= big){
								big = abs(A(j,k));
								irow = j;
								icol = k;
							}
						}
					}
				}
			}
			if (big <= T{1e-14}){
				throw std::runtime_error("utill:inplace_inverse('Gauss-Jordan' - Singular Matrix");
			}
			ipiv[icol]++;
			if (irow != icol){
				for (uint64_t l = 0; l < rows; l++){ // SWAP 
					temp = A(irow,l);
					A(irow,l) = A(icol,l);
					A(icol,l) = temp;
				}
				for (uint64_t l = 0; l < cols; l++){ // SWAP temp matrix
					temp = B(irow,l);
					B(irow,l) = B(icol,l);
					B(icol,l) = temp;
				}
			}
			indxr[i] = irow;
			indxc[i] = icol;
			if (A(icol,icol) == 0.0){
				throw std::runtime_error("utill:inplace_inverse('Gauss-Jordan' - Singular Matrix");
			}
			pivinv= 1.0/A(icol,icol);
			A(icol,icol)=1.0;
		
			for (uint64_t l = 0; l < rows; l++){
				A(icol,l) *= pivinv;
			}
			for (uint64_t l = 0; l < cols; l++){
				B(icol,l) *= pivinv;
			}
			for (uint64_t ll = 0; ll < rows; ll++){
				if (ll != icol){
					dum = A(ll,icol);
					A(ll,icol) = 0;
					for (uint64_t l = 0; l < rows; l++){
						A(ll,l) -= A(icol,l)*dum;
					}
					for (uint64_t l = 0; l < rows; l++){
						B(ll,l) -= B(icol,l)*dum;
					}
				}
			}

		}
		//m = temp_m;
		for (int64_t l = rows-1; l >= 0; l--){
			if (indxr[l] != indxc[l]){
				for (uint64_t k = 0; k < rows; k++){
					temp = A(k,indxr[l]);
				A(k,indxr[l]) = A(k,indxc[l]);
				A(k,indxc[l]) = temp;
				}
			}
		}
	}

} // namespace numerics

#endif // _inverse_gj_n_