/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          https://www.mrpt.org/                         |
   |                                                                        |
   | Copyright (c) 2005-2024, Individual contributors, see AUTHORS file     |
   | See: https://www.mrpt.org/Authors - All rights reserved.               |
   | Released under BSD License. See: https://www.mrpt.org/License          |
   +------------------------------------------------------------------------+ */
#pragma once

#include <mrpt/core/exceptions.h>
#include <mrpt/math/MatrixVectorBase.h>
#include <mrpt/system/datetime.h>

#include <Eigen/Dense>
#include <cstdint>
#include <cstdio>   // fopen(),...
#include <fstream>  // ifstream
#include <sstream>  // stringstream
#include <stdexcept>
#include <vector>

namespace mrpt::math
{
template <typename Scalar, class Derived>
bool MatrixVectorBase<Scalar, Derived>::fromMatlabStringFormat(
    const std::string& s, mrpt::optional_ref<std::ostream> dump_errors_here)
{
  // Start with a (0,0) matrix:
  if (Derived::RowsAtCompileTime == Eigen::Dynamic) mvbDerived().resize(0, 0);

  // Look for starting "[".
  size_t ini = s.find_first_not_of(" \t\r\n");
  if (ini == std::string::npos || s[ini] != '[')
  {
    return false;
  }

  size_t end = s.find_last_not_of(" \t\r\n");
  if (end == std::string::npos || s[end] != ']') return false;

  if (ini > end) return false;

  std::vector<Scalar> lstElements;

  size_t i = ini + 1;
  size_t nRow = 0;

  while (i < end)
  {
    // Extract one row:
    size_t end_row = s.find_first_of(";]", i);
    if (end_row == std::string::npos)
    {
      return false;
    }

    // We have one row in s[ i : (end_row-1) ]
    std::stringstream ss(s.substr(i, end_row - i));
    lstElements.clear();
    try
    {
      while (!ss.eof())
      {
        Scalar val;
        ss >> val;
        if (ss.bad() || ss.fail()) break;
        lstElements.push_back(val);
      }
    }
    catch (...)
    {
    }  // end of line

    // Empty row? Only for the first row, then this is an empty matrix:
    if (lstElements.empty())
    {
      if (nRow > 0)
        return false;
      else
      {
        // Else, this may be an empty matrix... if there is no next row,
        // we'll return with a (0,0) matrix
        if (Derived::RowsAtCompileTime == Eigen::Dynamic) mvbDerived().setZero(0, 0);
      }
    }
    else
    {
      const size_t N = lstElements.size();

      // Check valid width: All rows must have the same width
      if ((nRow > 0 && size_t(mvbDerived().cols()) != N) ||
          (nRow == 0 && Derived::ColsAtCompileTime != Eigen::Dynamic &&
           Derived::ColsAtCompileTime != int(N)))
      {
        if (dump_errors_here)
          dump_errors_here->get() << "[fromMatlabStringFormat] Row " << nRow + 1
                                  << " has invalid number of columns.\n";
        return false;
      }

      // Append to the matrix:
      if (Derived::RowsAtCompileTime == Eigen::Dynamic ||
          Derived::ColsAtCompileTime == Eigen::Dynamic)
      {
        mvbDerived().resize(nRow + 1, N);
      }
      else if (
          Derived::RowsAtCompileTime != Eigen::Dynamic && int(nRow) >= Derived::RowsAtCompileTime)
      {
        if (dump_errors_here)
          dump_errors_here->get() << "[fromMatlabStringFormat] Read more "
                                     "rows than the capacity of the "
                                     "fixed sized matrix.\n";
        return false;
      }
      for (size_t q = 0; q < N; q++) mvbDerived()(nRow, q) = lstElements[q];
      // Go for the next row:
      nRow++;
    }
    i = end_row + 1;
  }
  // For fixed sized matrices, check size:
  if (Derived::RowsAtCompileTime != Eigen::Dynamic && int(nRow) != Derived::RowsAtCompileTime)
  {
    if (dump_errors_here)
      dump_errors_here->get() << "[fromMatlabStringFormat] Read less rows "
                                 "than the capacity of the fixed sized "
                                 "matrix.\n";
    return false;
  }
  return true;  // Ok
}

template <typename Scalar, class Derived>
std::string MatrixVectorBase<Scalar, Derived>::inMatlabFormat(
    const std::size_t decimal_digits) const
{
  using Index = typename Derived::Index;
  std::stringstream s;
  s << "[" << std::scientific;
  s.precision(decimal_digits);
  for (Index i = 0; i < mvbDerived().rows(); i++)
  {
    for (Index j = 0; j < mvbDerived().cols(); j++) s << mvbDerived().coeff(i, j) << " ";
    if (i < mvbDerived().rows() - 1) s << ";";
  }
  s << "]";
  return s.str();
}

template <typename Scalar, class Derived>
void MatrixVectorBase<Scalar, Derived>::saveToTextFile(
    const std::string& file,
    mrpt::math::TMatrixTextFileFormat fileFormat,
    bool appendMRPTHeader,
    const std::string& userHeader) const
{
  using Index = typename Derived::Index;
  // Use a secure version in Visual Studio 2005+
#if defined(_MSC_VER) && (_MSC_VER >= 1400)
  FILE* f;
  if (0 != ::fopen_s(&f, file.c_str(), "wt")) f = nullptr;
#else
  FILE* f = ::fopen(file.c_str(), "wt");
#endif
  if (!f)
    throw std::runtime_error(
        std::string("saveToTextFile: Error opening file ") + file +
        std::string("' for writing a matrix as text."));

  if (!userHeader.empty()) fprintf(f, "%s", userHeader.c_str());

  if (appendMRPTHeader)
  {
    fprintf(
        f,
        "%% File generated with mrpt-math at %s\n"
        "%%-----------------------------------------------------------\n",
        mrpt::system::dateTimeLocalToString(mrpt::Clock::now()).c_str());
  }

  const auto& m = mvbDerived();
  for (Index i = 0; i < m.rows(); i++)
  {
    for (Index j = 0; j < m.cols(); j++)
    {
      switch (fileFormat)
      {
        case mrpt::math::MATRIX_FORMAT_ENG:
          ::fprintf(f, "%.16e", static_cast<double>(m(i, j)));
          break;
        case mrpt::math::MATRIX_FORMAT_FIXED:
          ::fprintf(f, "%.16f", static_cast<double>(m(i, j)));
          break;
        case mrpt::math::MATRIX_FORMAT_INT:
          ::fprintf(f, "%i", static_cast<int>(m(i, j)));
          break;
        default:
          throw std::runtime_error("Unsupported value for the parameter 'fileFormat'!");
      };
      // Separating blank space
      if (j < (mvbDerived().cols() - 1)) ::fprintf(f, " ");
    }
    ::fprintf(f, "\n");
  }
  ::fclose(f);
}

template <typename Scalar, class Derived>
void MatrixVectorBase<Scalar, Derived>::loadFromTextFile(std::istream& f)
{
  using Index = typename Derived::Index;
  std::string str;
  std::vector<double> fil(512);
  size_t nRows = 0;
  while (!f.eof() && !f.fail())
  {
    std::getline(f, str);
    if (str.size() && str[0] != '#' && str[0] != '%')
    {
      // Parse row to floats:
      const char* ptr = str.c_str();
      char* ptrEnd = nullptr;
      size_t i = 0;
      // Process each number in this row:
      while (ptr[0] && ptr != ptrEnd)
      {
        // Find next number: (non white-space character):
        while (ptr[0] && (ptr[0] == ' ' || ptr[0] == ',' || ptr[0] == '\t' || ptr[0] == '\r' ||
                          ptr[0] == '\n'))
          ptr++;
        if (fil.size() <= i) fil.resize(fil.size() + (fil.size() >> 1));
        // Convert to "double":
        fil[i] = strtod(ptr, &ptrEnd);
        // A valid conversion has been done?
        if (ptr != ptrEnd)
        {
          i++;  // Yes
          ptr = ptrEnd;
          ptrEnd = nullptr;
        }
      };  // end while procesing this row

      if (!i && nRows == 0) throw std::runtime_error("loadFromTextFile: Empty first line!");

      // "i": # of columns:
      if ((Derived::ColsAtCompileTime != Eigen::Dynamic && Index(i) != Derived::ColsAtCompileTime))
        throw std::runtime_error(
            "loadFromTextFile: The matrix in the text file does not "
            "match fixed matrix size");
      if (Derived::ColsAtCompileTime == Eigen::Dynamic && nRows > 0 &&
          Index(i) != mvbDerived().cols())
        throw std::runtime_error(
            "loadFromTextFile: The matrix in the text file does not "
            "have the same number of columns in all rows");

      // Append to the matrix:
      if (Derived::RowsAtCompileTime == Eigen::Dynamic ||
          Derived::ColsAtCompileTime == Eigen::Dynamic)
      {
        if (mvbDerived().rows() < static_cast<int>(nRows + 1) ||
            mvbDerived().cols() < static_cast<int>(i))
        {
          const size_t extra_rows = std::max(static_cast<size_t>(1), nRows >> 1);
          mvbDerived().resize(nRows + extra_rows, i);
        }
      }
      else if (
          Derived::RowsAtCompileTime != Eigen::Dynamic && int(nRows) >= Derived::RowsAtCompileTime)
        throw std::runtime_error(
            "loadFromTextFile: Read more rows than the capacity of the "
            "fixed sized matrix.");

      for (size_t q = 0; q < i; q++) mvbDerived()(nRows, q) = Scalar(fil[q]);

      nRows++;
    }  // end if fgets
  }    // end while not feof

  // Final resize to the real size (in case we allocated space in advance):
  if (Derived::RowsAtCompileTime == Eigen::Dynamic || Derived::ColsAtCompileTime == Eigen::Dynamic)
    mvbDerived().resize(nRows, mvbDerived().cols());

  // Report error as exception
  if (!nRows) throw std::runtime_error("loadFromTextFile: Error loading from text file");
}

template <typename Scalar, class Derived>
void MatrixVectorBase<Scalar, Derived>::loadFromTextFile(const std::string& file)
{
  std::ifstream f(file.c_str());
  if (f.fail()) throw std::runtime_error(std::string("loadFromTextFile: can't open file:") + file);
  loadFromTextFile(f);
}

template <typename Scalar, class Derived>
std::string MatrixVectorBase<Scalar, Derived>::asString() const
{
  std::stringstream ss;
  ss << mvbDerived().asEigen();
  return ss.str();
}

template <typename Scalar, class Derived>
Scalar MatrixVectorBase<Scalar, Derived>::sum() const
{
  return mvbDerived().asEigen().array().sum();
}

template <typename Scalar, class Derived>
Scalar MatrixVectorBase<Scalar, Derived>::sum_abs() const
{
  return mvbDerived().asEigen().array().abs().sum();
}

template <typename Scalar, class Derived>
Scalar MatrixVectorBase<Scalar, Derived>::minCoeff() const
{
  return mvbDerived().asEigen().minCoeff();
}

template <typename Scalar, class Derived>
Scalar MatrixVectorBase<Scalar, Derived>::maxCoeff() const
{
  return mvbDerived().asEigen().maxCoeff();
}

template <typename Scalar, class Derived>
Scalar MatrixVectorBase<Scalar, Derived>::minCoeff(std::size_t& outIdx) const
{
  if constexpr (Derived::ColsAtCompileTime == 1)
  {
    typename Derived::Index idx;
    auto r = mvbDerived().asEigen().minCoeff(&idx);
    outIdx = static_cast<std::size_t>(idx);
    return r;
  }
  else
    throw std::runtime_error("minCoeff(idx): Signature only valid for column vectors");
}

template <typename Scalar, class Derived>
Scalar MatrixVectorBase<Scalar, Derived>::maxCoeff(std::size_t& outIdx) const
{
  if constexpr (Derived::ColsAtCompileTime == 1)
  {
    typename Derived::Index idx;
    auto r = mvbDerived().asEigen().maxCoeff(&idx);
    outIdx = static_cast<std::size_t>(idx);
    return r;
  }
  else
    throw std::runtime_error("minCoeff(idx): Signature only valid for column vectors");
}
template <typename Scalar, class Derived>
Scalar MatrixVectorBase<Scalar, Derived>::minCoeff(std::size_t& rowIdx, std::size_t& colIdx) const
{
  typename Derived::Index row, col;
  auto r = mvbDerived().asEigen().minCoeff(&row, &col);
  rowIdx = static_cast<std::size_t>(row);
  colIdx = static_cast<std::size_t>(col);
  return r;
}

template <typename Scalar, class Derived>
Scalar MatrixVectorBase<Scalar, Derived>::maxCoeff(std::size_t& rowIdx, std::size_t& colIdx) const
{
  typename Derived::Index row, col;
  auto r = mvbDerived().asEigen().maxCoeff(&row, &col);
  rowIdx = static_cast<std::size_t>(row);
  colIdx = static_cast<std::size_t>(col);
  return r;
}

template <typename Scalar, class Derived>
void MatrixVectorBase<Scalar, Derived>::operator+=(Scalar s)
{
  mvbDerived().asEigen().array() += s;
}

template <typename Scalar, class Derived>
void MatrixVectorBase<Scalar, Derived>::operator-=(Scalar s)
{
  mvbDerived().asEigen().array() -= s;
}

template <typename Scalar, class Derived>
void MatrixVectorBase<Scalar, Derived>::operator*=(Scalar s)
{
  mvbDerived().asEigen().array() *= s;
}

template <typename Scalar, class Derived>
CMatrixDynamic<Scalar> MatrixVectorBase<Scalar, Derived>::operator*(const CMatrixDynamic<Scalar>& v)
{
  return CMatrixDynamic<Scalar>((mvbDerived().asEigen() * v.asEigen()).eval());
}

template <typename Scalar, class Derived>
Derived MatrixVectorBase<Scalar, Derived>::impl_op_add(const Derived& m2) const
{
  Derived ret(mvbDerived().rows(), mvbDerived().cols());
  ret.asEigen() = mvbDerived().asEigen() + m2.asEigen();
  return ret;
}
template <typename Scalar, class Derived>
void MatrixVectorBase<Scalar, Derived>::impl_op_selfadd(const Derived& m2)
{
  mvbDerived().asEigen() += m2.asEigen();
}
template <typename Scalar, class Derived>
Derived MatrixVectorBase<Scalar, Derived>::impl_op_subs(const Derived& m2) const
{
  Derived ret(mvbDerived().rows(), mvbDerived().cols());
  ret.asEigen() = mvbDerived().asEigen() - m2.asEigen();
  return ret;
}
template <typename Scalar, class Derived>
void MatrixVectorBase<Scalar, Derived>::impl_op_selfsubs(const Derived& m2)
{
  mvbDerived().asEigen() -= m2.asEigen();
}
template <typename Scalar, class Derived>
Derived MatrixVectorBase<Scalar, Derived>::operator*(const Derived& m2) const
{
  ASSERTMSG_(
      mvbDerived().cols() == mvbDerived().rows(),
      "Operator* implemented only for square matrices. Use `A.asEigen() * "
      "B.asEigen()` for general matrix products.");
  Derived ret(mvbDerived().rows(), mvbDerived().rows());
  if constexpr (Derived::RowsAtCompileTime == Derived::ColsAtCompileTime)
  {
    ret.asEigen() = mvbDerived().asEigen() * m2.asEigen();
  }
  return ret;
}

template <typename Scalar, class Derived>
void MatrixVectorBase<Scalar, Derived>::matProductOf_Ab(
    const CMatrixDynamic<Scalar>& A, const CVectorDynamic<Scalar>& b)
{
  mvbDerived() = A.asEigen() * b.asEigen();
}

template <typename Scalar, class Derived>
void MatrixVectorBase<Scalar, Derived>::matProductOf_Atb(
    const CMatrixDynamic<Scalar>& A, const CVectorDynamic<Scalar>& b)
{
  mvbDerived() = A.asEigen().transpose() * b.asEigen();
}

template <typename Scalar, class Derived>
Scalar MatrixVectorBase<Scalar, Derived>::norm_inf() const
{
  return mvbDerived().asEigen().template lpNorm<Eigen::Infinity>();
}

template <typename Scalar, class Derived>
Scalar MatrixVectorBase<Scalar, Derived>::norm() const
{
  return mvbDerived().asEigen().norm();
}

template <typename Scalar, class Derived>
Scalar MatrixVectorBase<Scalar, Derived>::dot(const CVectorDynamic<Scalar>& v) const
{
  if constexpr (Derived::ColsAtCompileTime == 1)
  {
    return mvbDerived().asEigen().dot(v.mvbDerived().asEigen());
  }
  else
  {
    ASSERTMSG_(false, "dot(): Implemented for column vectors only.");
  }
}
template <typename Scalar, class Derived>
Scalar MatrixVectorBase<Scalar, Derived>::dot(const MatrixVectorBase<Scalar, Derived>& v) const
{
  if constexpr (Derived::ColsAtCompileTime == 1)
  {
    return mvbDerived().asEigen().dot(v.mvbDerived().asEigen());
  }
  else
  {
    ASSERTMSG_(false, "dot(): Implemented for column vectors only.");
  }
}

}  // namespace mrpt::math
