/* Copyright (c) 1997-2023
   Ewgenij Gawrilow, Michael Joswig, and the polymake team
   Technische Universität Berlin, Germany
   https://polymake.org

   This program is free software; you can redistribute it and/or modify it
   under the terms of the GNU General Public License as published by the
   Free Software Foundation; either version 2, or (at your option) any
   later version: http://www.gnu.org/licenses/gpl.txt.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.
--------------------------------------------------------------------------------
*/

#include <cmath>
#include "polymake/Matrix.h"
#include "polymake/linalg.h"
#include <algorithm>

namespace pm {

Matrix<double> inv(Matrix<double> m)
{
   const Int dim = m.rows();
   std::vector<Int> row_index(dim);
   copy_range(entire(sequence(0, dim)), row_index.begin());
   Matrix<double> u = unit_matrix<double>(dim);
   const double epsilon = 1e-8;

   for (Int c = 0; c < dim; ++c) {
      Int r = 0;
      double max_pivot = 0;
      for (Int rr = c; rr < dim; ++rr) {
         const double p = abs(m(row_index[rr], c));
         if (p > max_pivot) {
            r = rr;
            max_pivot = p;
         }
      }
      if (max_pivot > epsilon) {
         double* ppivot = &m(row_index[r], c);
         const double pivot = *ppivot;
         double* urow = &u(row_index[r], 0);
         if (r != c) std::swap(row_index[r], row_index[c]);
         if (pivot != 1) {
            double* e = ppivot;
            for (Int i = c+1; i < dim; ++i) (*++e)/=pivot;
            for (Int i = 0; i <= c; ++i) urow[row_index[i]] /= pivot;
         }
         for (r = 0; r < dim; ++r) {
            if (r == c) continue;
            double* e2 = &m(row_index[r], c);
            const double factor = *e2;
            if (abs(factor) > epsilon) {
               double* e = ppivot;
               for (Int i = c+1; i < dim; ++i) (*++e2) -= (*++e) * factor;
               double* urow2 = &u(row_index[r], 0);
               for (Int i = 0; i <= c; ++i) urow2[row_index[i]] -= urow[row_index[i]] * factor;
            }
         }
      } else {
         throw degenerate_matrix();
      }
   }
   return Matrix<double>(dim, dim, select(rows(u),row_index).begin());
}

} // end namespace pm

// Local Variables:
// mode:C++
// c-basic-offset:3
// indent-tabs-mode:nil
// End:
