#ifndef SOPUHS_TESTS_MACROS_HPP
#define SOPUHS_TESTS_MACROS_HPP

#include <sophus/types.hpp>
#include <sophus/formatstring.hpp>

namespace Sophus {
namespace details {

template <class Scalar>
class Pretty {
 public:
  static std::string impl(Scalar s) { return FormatString("%", s); }
};

template <class Scalar, int M, int N>
class Pretty<Eigen::Matrix<Scalar, M, N>> {
 public:
  static std::string impl(Matrix<Scalar, M, N> const& v) {
    return FormatString("\n%\n", v);
  }
};

template <class T>
std::string pretty(T const& v) {
  return Pretty<T>::impl(v);
}

template <class... Args>
void testFailed(bool& passed, char const* func, char const* file, int line,
                std::string const& msg) {
  std::cerr << FormatString("Test failed in function %, file %, line %\n", func,
                            file, line);
  std::cerr << msg << "\n\n";
  passed = false;
}
}  // namespace details

void processTestResult(bool passed) {
  if (!passed) {
    // LCOV_EXCL_START
    std::cerr << "failed!" << std::endl << std::endl;
    exit(-1);
    // LCOV_EXCL_STOP
  }
  std::cerr << "passed." << std::endl << std::endl;
}
}  // namespace Sophus

#define SOPHUS_STRINGIFY(x) #x

/// GenericTests whether condition is true.
/// The in-out parameter passed will be set to false if test fails.
#define SOPHUS_TEST(passed, condition, ...)                                    \
  do {                                                                         \
    if (!(condition)) {                                                        \
      std::string msg = Sophus::details::FormatString(                         \
          "condition ``%`` is false\n", SOPHUS_STRINGIFY(condition));          \
      msg += Sophus::details::FormatString(__VA_ARGS__);                       \
      Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
                                  msg);                                        \
    }                                                                          \
  } while (false)

/// GenericTests whether left is equal to right given a threshold.
/// The in-out parameter passed will be set to false if test fails.
#define SOPHUS_TEST_EQUAL(passed, left, right, ...)                            \
  do {                                                                         \
    if (left != right) {                                                       \
      std::string msg = Sophus::details::FormatString(                         \
          "% (=%) is not equal to % (=%)\n", SOPHUS_STRINGIFY(left),           \
          Sophus::details::pretty(left), SOPHUS_STRINGIFY(right),              \
          Sophus::details::pretty(right));                                     \
      msg += Sophus::details::FormatString(__VA_ARGS__);                       \
      Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
                                  msg);                                        \
    }                                                                          \
  } while (false)

/// GenericTests whether left is equal to right given a threshold.
/// The in-out parameter passed will be set to false if test fails.
#define SOPHUS_TEST_NEQ(passed, left, right, ...)                              \
  do {                                                                         \
    if (left == right) {                                                       \
      std::string msg = Sophus::details::FormatString(                         \
          "% (=%) shoudl not be equal to % (=%)\n", SOPHUS_STRINGIFY(left),    \
          Sophus::details::pretty(left), SOPHUS_STRINGIFY(right),              \
          Sophus::details::pretty(right));                                     \
      msg += Sophus::details::FormatString(__VA_ARGS__);                       \
      Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
                                  msg);                                        \
    }                                                                          \
  } while (false)

/// GenericTests whether left is approximatly equal to right given a threshold.
/// The in-out parameter passed will be set to false if test fails.
#define SOPHUS_TEST_APPROX(passed, left, right, thr, ...)                      \
  do {                                                                         \
    auto nrm = Sophus::maxMetric((left), (right));                             \
    if (!(nrm < (thr))) {                                                      \
      std::string msg = Sophus::details::FormatString(                         \
          "% (=%) is not approx % (=%); % is %; nrm is %\n",                   \
          SOPHUS_STRINGIFY(left), Sophus::details::pretty(left),               \
          SOPHUS_STRINGIFY(right), Sophus::details::pretty(right),             \
          SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm);           \
      msg += Sophus::details::FormatString(__VA_ARGS__);                       \
      Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
                                  msg);                                        \
    }                                                                          \
  } while (false)

/// GenericTests whether left is NOT approximatly equal to right given a
/// threshold. The in-out parameter passed will be set to false if test fails.
#define SOPHUS_TEST_NOT_APPROX(passed, left, right, thr, ...)                  \
  do {                                                                         \
    auto nrm = Sophus::maxMetric((left), (right));                             \
    if (nrm < (thr)) {                                                         \
      std::string msg = Sophus::details::FormatString(                         \
          "% (=%) is approx % (=%), but it should not!\n % is %; nrm is %\n",  \
          SOPHUS_STRINGIFY(left), Sophus::details::pretty(left),               \
          SOPHUS_STRINGIFY(right), Sophus::details::pretty(right),             \
          SOPHUS_STRINGIFY(thr), Sophus::details::pretty(thr), nrm);           \
      msg += Sophus::details::FormatString(__VA_ARGS__);                       \
      Sophus::details::testFailed(passed, SOPHUS_FUNCTION, __FILE__, __LINE__, \
                                  msg);                                        \
    }                                                                          \
  } while (false)

#endif  // SOPUHS_TESTS_MACROS_HPP