From dea841103da96ba3627a4af6ebd76280f3a61a4d Mon Sep 17 00:00:00 2001
From: Tyler Veness <calcmogul@gmail.com>
Date: Sat, 5 Jun 2021 21:10:41 -0700
Subject: [PATCH] [wpimath] Add fmtlib formatter overloads for Eigen::Matrix
 and units (#3409)

This allows using Eigen matrices or units natively with fmt::format() or
fmt::print().
---
 .../src/main/native/include/frc/fmt/Eigen.h   |  44 ++++
 .../src/main/native/include/frc/fmt/Units.h   | 212 ++++++++++++++++++
 wpimath/src/test/native/cpp/FormatterTest.cpp |  24 ++
 3 files changed, 280 insertions(+)
 create mode 100644 wpimath/src/main/native/include/frc/fmt/Eigen.h
 create mode 100644 wpimath/src/main/native/include/frc/fmt/Units.h
 create mode 100644 wpimath/src/test/native/cpp/FormatterTest.cpp

diff --git a/wpimath/src/main/native/include/frc/fmt/Eigen.h b/wpimath/src/main/native/include/frc/fmt/Eigen.h
new file mode 100644
index 0000000000..c78b597a92
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/fmt/Eigen.h
@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <fmt/format.h>
+
+#include "Eigen/Core"
+
+template <int Rows, int Cols, int... Args>
+struct fmt::formatter<Eigen::Matrix<double, Rows, Cols, Args...>> {
+  char presentation = 'f';
+
+  constexpr auto parse(fmt::format_parse_context& ctx) {
+    auto it = ctx.begin(), end = ctx.end();
+    if (it != end && (*it == 'f' || *it == 'e')) {
+      presentation = *it++;
+    }
+
+    if (it != end && *it != '}') {
+      throw fmt::format_error("invalid format");
+    }
+
+    return it;
+  }
+
+  template <typename FormatContext>
+  auto format(const Eigen::Matrix<double, Rows, Cols, Args...>& mat,
+              FormatContext& ctx) {
+    auto out = ctx.out();
+    for (int i = 0; i < Rows; ++i) {
+      for (int j = 0; j < Cols; ++j) {
+        out = fmt::format_to(out, "  {:f}", mat(i, j));
+      }
+
+      if (i < Rows - 1) {
+        out = fmt::format_to(out, "\n");
+      }
+    }
+
+    return out;
+  }
+};
diff --git a/wpimath/src/main/native/include/frc/fmt/Units.h b/wpimath/src/main/native/include/frc/fmt/Units.h
new file mode 100644
index 0000000000..ccae610cfb
--- /dev/null
+++ b/wpimath/src/main/native/include/frc/fmt/Units.h
@@ -0,0 +1,212 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <fmt/format.h>
+
+#include "units/base.h"
+
+template <class Units, typename T, template <typename> class NonLinearScale>
+struct fmt::formatter<units::unit_t<Units, T, NonLinearScale>> {
+  char presentation = 'f';
+
+  constexpr auto parse(fmt::format_parse_context& ctx) {
+    auto it = ctx.begin(), end = ctx.end();
+    if (it != end && (*it == 'f' || *it == 'e')) {
+      presentation = *it++;
+    }
+
+    if (it != end && *it != '}') {
+      throw fmt::format_error("invalid format");
+    }
+
+    return it;
+  }
+
+  template <typename FormatContext>
+  auto format(const units::unit_t<Units, T, NonLinearScale>& obj,
+              FormatContext& ctx) {
+    using BaseUnits =
+        units::unit<std::ratio<1>,
+                    typename units::traits::unit_traits<Units>::base_unit_type>;
+
+    auto out = ctx.out();
+
+    out = fmt::format_to(out, "{}", units::convert<Units, BaseUnits>(obj()));
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::meter_ratio::num != 0) {
+      out = fmt::format_to(out, " m");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::meter_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::meter_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::meter_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::meter_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::meter_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::kilogram_ratio::num != 0) {
+      out = fmt::format_to(out, " kg");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::kilogram_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::kilogram_ratio::num != 1) {
+      out = fmt::format_to(out, "^{}",
+                           units::traits::unit_traits<
+                               Units>::base_unit_type::kilogram_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::kilogram_ratio::den != 1) {
+      out = fmt::format_to(out, "/{}",
+                           units::traits::unit_traits<
+                               Units>::base_unit_type::kilogram_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::second_ratio::num != 0) {
+      out = fmt::format_to(out, " s");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::second_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::second_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::second_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::second_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::second_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::ampere_ratio::num != 0) {
+      out = fmt::format_to(out, " A");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::ampere_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::ampere_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::ampere_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::ampere_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::ampere_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::kelvin_ratio::num != 0) {
+      out = fmt::format_to(out, " K");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::kelvin_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::kelvin_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::kelvin_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::kelvin_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::kelvin_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::mole_ratio::num != 0) {
+      out = fmt::format_to(out, " mol");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::mole_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::mole_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::mole_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::mole_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::mole_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::candela_ratio::num != 0) {
+      out = fmt::format_to(out, " cd");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::candela_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::candela_ratio::num != 1) {
+      out = fmt::format_to(out, "^{}",
+                           units::traits::unit_traits<
+                               Units>::base_unit_type::candela_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::candela_ratio::den != 1) {
+      out = fmt::format_to(out, "/{}",
+                           units::traits::unit_traits<
+                               Units>::base_unit_type::candela_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::radian_ratio::num != 0) {
+      out = fmt::format_to(out, " rad");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::radian_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::radian_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::radian_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::radian_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::radian_ratio::den);
+    }
+
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::byte_ratio::num != 0) {
+      out = fmt::format_to(out, " b");
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::byte_ratio::num != 0 &&
+                  units::traits::unit_traits<
+                      Units>::base_unit_type::byte_ratio::num != 1) {
+      out = fmt::format_to(
+          out, "^{}",
+          units::traits::unit_traits<Units>::base_unit_type::byte_ratio::num);
+    }
+    if constexpr (units::traits::unit_traits<
+                      Units>::base_unit_type::byte_ratio::den != 1) {
+      out = fmt::format_to(
+          out, "/{}",
+          units::traits::unit_traits<Units>::base_unit_type::byte_ratio::den);
+    }
+
+    return out;
+  }
+};
diff --git a/wpimath/src/test/native/cpp/FormatterTest.cpp b/wpimath/src/test/native/cpp/FormatterTest.cpp
new file mode 100644
index 0000000000..6828b7d7a0
--- /dev/null
+++ b/wpimath/src/test/native/cpp/FormatterTest.cpp
@@ -0,0 +1,24 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <fmt/format.h>
+
+#include "frc/fmt/Eigen.h"
+#include "frc/fmt/Units.h"
+#include "gtest/gtest.h"
+#include "units/velocity.h"
+
+TEST(FormatterTest, Eigen) {
+  Eigen::Matrix<double, 3, 2> A;
+  A << 1.0, 2.0, 3.0, 4.0, 5.0, 6.0;
+  EXPECT_EQ(
+      "  1.000000  2.000000\n"
+      "  3.000000  4.000000\n"
+      "  5.000000  6.000000",
+      fmt::format("{}", A));
+}
+
+TEST(FormatterTest, Units) {
+  EXPECT_EQ("4 m s^-1", fmt::format("{}", 4_mps));
+}